단안 카메라를 이용한 수중 정밀 항법을 위한 모델 기반 포즈 추정

1. 서 론

일반적으로 원격수중로봇(Remotely underwater vehicle) 을 이용한 수중 임무는 로봇에 탑재된 카메라로부터 얻어진 영상정보에 의존하여 운용자가 수동 조작을 통해 수중로봇 을 이동시키거나 로봇팔을 조작하는 방식으로 수행된다. 하 지만 이러한 운용 방법은 영상에서 3차원 공간을 추측하는 데 운용자의 경험에 전적으로 의존하게 되고 조류와 같은 환경 외란에 적절히 대응하기 어려워 수중 로봇의 정확한 운용에 한계가 존재한다. 이러한 이유로 원격수중로봇을 이 용한 작업에서 운용자의 개입을 최소로하며 운용 정확도를 높이기 위해 다양한 자율 알고리즘이 제안되어 왔다.

광학 센서의 경우 가시거리의 제약이 커서 수중에서의 활용에 제약을 받는 것이 사실이나 기존의 유인 조작자에 의한 수중로봇 조작의 많은 부분을 영상에 의존하는 것으로 감안할 때 자율 알고리즘의 구현에 있어서도 영상 정보를 효과적으로 활용하는 것이 중요하다.

이와 관련된 대표적인 연구로 수중 로봇이 소나 및 영상 정보를 이용하여 목표물을 자율 탐지 및 회수하는 시스템이 SAUVIM 프로젝트^[1]에서 제시된 바가 있으며 TRIDENT 프로젝트^[2]에서는 수중 로봇이 이미지 정합 기법을 이용하 여 해저면의 그림 지도를 자율로 작성하고 지도에서 확인된 목표물의 위치로 이동한 뒤 영상처리 알고리즘을 통해 목표 물을 자율 탐지 및 회수하는 시스템을 실험적으로 검증한 바 있다. TRITON 프로젝트^[3]에서는 수중 로봇이 이미지 기반 포즈 추정을 통해 얻어진 수중 구조물의 위치 정보를 기반으로 자율 도킹 및 밸드 조작을 수행한 바 있다. 이외에 도 수중 로봇이 제어판(Control panel)에 대해서 이미지 특 징점 기반 매칭을 통해 제어 패널의 포즈를 추정하고 제어 패널의 밸브를 자율로 조작하는 시나리오가 수조 환경에서 시도되었다^[4].

위에 제시된 연구 사례는 기존 추측 항법의 표류 오차를 보정하기 위해 영상 기반 위치 정보를 도입하였으나 그 기 능이 구조물의 국부적인 위치에 국한되므로 구조물의 전역 에서 활용되기에는 한계가 있다. 이 점에 주목하여 본 연구 에서는 수중 구조물 전역에서 활용이 가능한 이미지 기반 포즈 추정 기법이 고려된 모델 참조 수중 항법을 제안하고 이를 실시간 알고리즘으로 구현한 후 실제 수중 로봇을 활 용한 실험을 통해 실제적 적용 가능성을 확인하고자 한다. 항법 알고리즘의 실험적 검증을 위해 심해 유전 구조물인 웰-헤드(Well head)를 단순화한 모델 스케일 구조물로 실 험 환경을 구성하고, 원격수중로봇을 이용한 위치 이동 및 제어판 조작 임무를 설정한뒤 이를 자율로 수행함으로써 제안된 항법 알고리즘의 실제적 유용성을 보인다.

2. 모델 기반 포즈 추정 알고리즘

모델 기반 포즈 추정은 3차원 공간에서 카메라와 참조물 체 사이의 상대 포즈(위치 및 자세)를 추정하는 동시에 참조 물체를 추적하는 컴퓨터 비전 알고리즘이다. 본 기법은 참 조 대상이 되는 물체를 직선 또는 원과 같은 기하학적 기본 요소로 모델링한 3차원 CAD 모델을 이미지에 투영시키고 이를 실제 물체가 존재하는 곳에 정확히 위치시키기 위해 가상 시각 구동(VVS: Virtual Visual Servoing) 기법을 이 용한다^[5].

VVS의 제어 기법에서 목적에 따라 다양한 형태의 에러 가 제어의 대상이 될 수 있지만 본 연구에서는 투영된 3차원 모델 위의 점으로부터 모델에 대응되는 점까지의 픽셀 거리 를 오차로 한다. 따라서 오차를 정의하기 위해 모델에 대응 되는 대응점을 검색하는 과정이 요구되며 이를 위해 ME (Moving Edge) 알고리즘을 적용하였다^[6]. ME 알고리즘은 사전에 윤곽선 추출과 같은 작업을 요구하지 않으며 단지 이미지 픽셀 값을 대상으로하는 컨볼루션 연산(Convolution computation)을 이용하여 높은 연산 효율을 통한 실시간성 을 기대할 수 있으며 동시에 준수한 대응점 검색 성능을 보여준다. 본 절에서 다루어지는 VVS의 내용과 식의 전개 는 다섯 번째 참고문헌^[5]의 방식을 기초로 작성되었다.

2.1 가상 시각 구동

가상 시각 구동(VVS)은 가상 카메라(Virtual camera)가 특정 위치에서 참조 물체를 관찰하기 위해 가상 카메라의 위치를 제어하는 것이다. 가상 카메라의 제어는 이미지에 투영된 모델의 윤곽선 위의 N개의 점 s와 대응점 s^*사이의 픽셀 거리의 차이로부터 얻어진 오차 벡터를 정의하고, 이 를 최소화함으로써 이루어진다(Fig. 1)^[7]. 오차 벡터는 다음 과 같은 목적함수를 정의하는 것으로 구체화될 수 있다.

$\[ e=D\left(s\left(r\right)-s^{*}\right) \]$

(1)

Fig. 1

Model-based tracking schematic (left), error between rendered and referenced model (middle), pre-defined mask (right)

여기서 s(r)는 가상 카메라의 포즈 r에 의해 이미지 평면에 투영된 모델 위의 점이며 s^*는 ME 알고리즘에 의해 검색된 대응점이다. 투영된 모델의 윤곽선에서 고려되는 점의 수가 N일 경우 (s(r) - s^*)의 크기는 N× 1이 되고, D는 N× N 의 크기를 가진 가중 대각 행렬이며 각 성분은 각 에러의 신뢰도를 반영한다. D가 상수라면 식 (1)의 미분은 가상 카메라 속도(선속도와 각속도) v와 점 s에 대한 상관행렬 (Interaction matrix) L_s로 식 (2)와 같이 표현된다.

$\[ \dot{e}=\frac{\delta e}{\delta s}\frac{\delta s}{\delta r}\frac{\delta r}{\delta \text{t}}=D{L}_{s}v \]$

(2)

오차가 λ (λ > 0) 와 함께 식 (3)와 같이 감소한다면

$\[ \dot{e}=-\lambda e \]$

(3)

식 (2)는 가상 카메라 속도 v에 대해 정리되며 다음과 같은 제어 법칙을 얻을 수 있다.

$\[ v=-\lambda {\left(\widehat{D}{\widehat{L}}_s\right)}^{+}D\left(s\left(r\right)-s^{*}\right) \]$

(4)

여기서 ( $$ \widehat{D}{\widehat{L}}_{\text{s}}$$) 는 $$ \widehat{D}{\widehat{L}}_{\text{s}}$$의 준역행렬이며 $$ \widehat{D}{\widehat{L}}_{\text{s}}$$는 DL_s의 추 정값이다. 여기서 얻어진 가상 카메라 속도 v를 포즈 갱신 에 반영하기 위해 로드리게스 식(Rodrigues’s formular)이 이용되며 카메라와 물체 사이의 포즈를 갱신하기 위해 exponential map이 적용된 형태는 식 (5)와 같다.

$\[ {}^{\text{c}}{\text{M}}_{\text{o}}{}^{\text{k+1}}\text{=}{\hspace{0.17em}}^{\text{c}}{M}_{\text{o}}{}^{\text{k}}{\text{e}}^{\text{v}} \]$

(5)

여기서 e^v는 v에 대한 exponential map이며 ^cM_o는 3차원 공간에서 물체 좌표계에서 정의되는 카메라의 포즈, 그리고 k는 반복 횟수이다.

3. 모델 참조 수중 항법 알고리즘

3.2 확장 칼만 필터

수중 운동체와 구조물의 포즈 추정 및 센서 융합을 위해 비선형 추정 필터인 확장 칼만 필터를 이용한다.

3.2.1 운동 모델

수중 운동체는 3차원 공간에서 6자유도 운동을 하며 이에 따른 기구학 모델은 식 (8)과 같다.

$\[ \begin{array}{l}\dot{x}=u\hspace{0.17em}\text{cos}\theta \hspace{0.17em}\text{cos}\psi +\upsilon (\text{sin}\hspace{0.17em}\varphi \hspace{0.17em}\text{sin}\theta \hspace{0.17em}\text{cos}\psi \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{0.17em}\hspace{0.17em}-\text{cos}\varphi \text{sin}\psi )\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{0.17em}\hspace{0.17em}+w(\text{cos}\varphi \text{sin}\theta \text{cos}\psi \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{0.17em}\hspace{0.17em}+\text{sin}\varphi \text{sin}\psi )\\ \dot{y}=u\hspace{0.17em}\text{cos}\theta \hspace{0.17em}\text{sin}\psi +\upsilon (\text{sin}\hspace{0.17em}\varphi \hspace{0.17em}\text{sin}\theta \hspace{0.17em}\text{sin}\psi \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{0.17em}\hspace{0.17em}\text{+cos}\varphi \hspace{0.17em}\text{cos}\psi )\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{0.17em}\hspace{0.17em}+w(\text{cos}\varphi \hspace{0.17em}\text{sin}\theta \hspace{0.17em}\text{sin}\psi \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{0.17em}\hspace{0.17em}-\text{sin}\varphi \hspace{0.17em}\text{cos}\psi )\\ \dot{z}=-u\hspace{0.17em}\text{sin}\theta +\upsilon \hspace{0.17em}\text{sin}\varphi \hspace{0.17em}\text{cos}\theta +w\hspace{0.17em}\text{cos}\varphi \hspace{0.17em}\text{cos}\theta \\ \dot{\varphi }=z_p+z_q\text{sin}\varphi \hspace{0.17em}\text{tan}\theta +z_r\cos \varphi \hspace{0.17em}\text{tan}\theta \\ \dot{\theta }=z_q\hspace{0.17em}\text{cos}\varphi -z_r\hspace{0.17em}\text{sin}\varphi \\ \dot{\psi }=z_q\frac{\text{sin}\varphi }{\text{cos}\theta }+z_r\frac{\text{cos}\varphi }{\text{cos}\theta }\\ \dot{u}=z_x-g\hspace{0.17em}\text{sin}\theta -z_{q}w+z_r\upsilon \\ \dot{\upsilon }=z_y-g\hspace{0.17em}\text{sin}\varphi \hspace{0.17em}\text{cos}\theta -z_{r}u+z_{p}w\\ \dot{w}=z_z-g\hspace{0.17em}\text{cos}\varphi \hspace{0.17em}\text{cos}\theta -z_p\upsilon +z_{q}u\\ {\dot{x}}_{m/r}={\dot{y}}_{m/r}={\dot{z}}_{m/r}=0\\ {\dot{\varphi }}_{m/r}={\dot{\theta }}_{m/r}={\dot{\psi }}_{m/r}=0\end{array} \]$

(8)

운동체와 수중 구조물의 포즈를 나타내는 시스템의 상태 벡터는 식 (9)와 같으며 운동체와 구조물의 위치는 동시에 추정하도록 구성되었다.

$\[ \begin{array}{c}x=\left[\begin{array}{c}{P}_{\text{v/r}}\\ P_{\text{m/r}}\\ U\end{array}\right]\\ =\left[\begin{array}{c}{x\hspace{0.17em}y\hspace{0.17em}z\hspace{0.17em}\varphi \hspace{0.17em}\theta \hspace{0.17em}\psi ]}^T\\ {\left[x_{m/r}{y}_{m/r}{z}_{m/r}{\varphi }_{m/r}{\theta }_{m/r}{\psi }_{m/r}\right]}^T\\ {\left[u\hspace{0.17em}\upsilon \hspace{0.17em}w\right]}^T\end{array}\right]\end{array} \]$

(9)

시스템 모델은 식 (10)으로 나타내어지고 구조물의 포즈 는 정적인 상태를 유지하는 것으로 가정하였다.

$\[ \dot{x}=\left[\begin{array}{c}{\dot{P}}_{\text{v/r}}\\ {\dot{P}}_{\text{m/r}}\\ \dot{U}\end{array}\right]=\left[\begin{array}{c}{f}_1\left(P_{\text{v/r}},z_{\text{imu}}\right)\\ 0\\ f_2\left(U,z_{\text{imu}}\right)\end{array}\right]+W \]$

(10)

Fig. 2

Model-referenced underwater navigations

Fig. 3

Coordinate systems in proposed navigation algorithm

Table 1

Pseudo-code of vehicle guidance

여기서 f₁ (∙) 와 f₂ (∙)는 6자유도 비선형 운동 모델과 동 체좌표계에서 표현되는 선속도의 모델이고, W는 환경 외란 을 반영하는 잡음을 나타낸다. 식 (11)은 관성측정장치 (IMU: Inertial Measurement Unit)로부터 계측되는 동체 좌표계에서의 가속도과 각속도를 나타낸다.

$\[ z_{imu}={\left[z_x\hspace{0.17em}{z}_y\hspace{0.17em}{z}_z\hspace{0.17em}{z}_p\hspace{0.17em}{z}_q\hspace{0.17em}{z}_r\right]}^T \]$

(11)

3.2.2 계측 모델

계측 모델에 반영되는 센서로 동체 좌표계에서 선속도를 계 측하는 도플러 속도계(DVL: Doppler Velocity Log), 자세 및 각속도 정보를 계측하는 자세 방위 장치(AHRS: Attitude Heading Reference System) 그리고 모델 기반 포즈 추정을 위한 카메라가 활용된다.

Fig. 4

KAIST test tank (left) and model scaled underwater well-head (right)

식 (12)에서 계측 모델 z의 구성 요소인 u, υ, w 는 DVL 로부터 얻어지는 동체좌표계에서의 운동체의 선속도이며 ϕ, θ, ψ는 AHRS로부터 얻어지는 운동체의 자세(roll, pitch, yaw) 각도를 나타낸다. z_m/υ는 운동체에 대한 수중 구조물의 상대 포즈이며 모델 기반 포즈 추정 알고리즘으로 부터 얻어지는 카메라에 대한 상대 포즈 z_mbt서 좌표 변환 을 통해 얻어진다. 마지막으로 V는 센서의 계측 잡음을 나 타낸다.

$\[ z={\left[z_{DVL}\hspace{0.17em}{z}_{AHRS}\hspace{0.17em}{z}_{m/\upsilon }\right]}^T+V \]$

(12)

$\[ z_{DVL}={\left[u\hspace{0.17em}\upsilon \hspace{0.17em}w\right]}^T,\hspace{0.17em}{z}_{AHRS}={\left[\varphi \hspace{0.17em}\theta \hspace{0.17em}\psi \right]}^T \]$

(13)

$\[ z_{m/\upsilon }=E\left[P_{c/\upsilon }\right]z_{mbt} \]$

(14)

$\[ z_{mbt}={\left[x_{m/c}\hspace{0.17em}{y}_{m/c}\hspace{0.17em}{z}_{m/c}\hspace{0.17em}{\varphi }_{m/c}\hspace{0.17em}{\theta }_{m/c}\hspace{0.17em}{\psi }_{m/c}\right]}^T \]$

(15)

4. 실험적 검증

모델 참조 수중 항법 알고리즘은 수중 운동체가 수중 구 조물 인근에서 자율 작업을 수행하기 위해 제안된 것으로 이러한 항법 알고리즘의 목적 및 적용 범위를 반영한 실험 시나리오가 선정되었다.

4.1 실험적 검증을 위한 시나리오

4.1.1 웰-헤드의 기능 조작 자율 수행

자율 조작 시나리오는 수중 유전 구조물인 웰-헤드 상부 에 설치되는 제어판을 수중 운동체가 자율로 조작하는 것을 가정하여 수중 운동체가 제어판 기구부 인근으로 이동하여 로봇팔을 대체해 수중 운동체의 전면부에 탑재된 조작 기구 를 이용해 슬라이딩 기구부를 좌측에서 우측으로 옮기는 작업을 수행하는 임무를 설정하였다.

4.1.2 수중 운동체의 작업 공간 자율 이동

공간 자율 이동 시나리오는 수중 구조물 인근에 위치한 수중 운동체가 현재 작업공간에서 다른 작업 공간으로 이동 하는 것으로 이동 과정에서는 운동체에 탑재된 카메라의 영상을 통해 구중 구조물의 현황을 지속적으로 파악할 수 있도록 카메라의 시야각(FOV: Field of view)에 구조물이 관찰될 수 있도록 한다.

Fig. 5

Underwater robot

Fig. 6

Configuration of proposed navigation system

4.4 실험 결과

모든 실험은 실시간으로 수행되었으며 임무 수행 과정에 서 별도의 사용자 개입을 배제한 자율 구동의 결과이다.

4.4.1 웰-헤드의 기능 조작 자율 수행

웰-헤드 자율 조작 실험에서 자율 수중 작업의 과정과 모델 기반 포즈 추정 결과를 Fig. 7에 보인다. 결과로부터 확인할 수 있듯이 웰-헤드 자율 조작 실험은 성공적으로 수행되었으며 일회성 결과가 아닌 수차례 시도에도 일관된 결과를 보여주었다.

Fig. 7

Snapshots and vehicle trajectories of the slide-bar manipulation

운용자가 관여하지 않는 웰-헤드 자율 조작 시나리오를 성공적으로 수행하기 위해서는 수 센티미터(centimeter) 이 내 오차의 매우 높은 항법 정밀도를 요구하기 때문에 제안 된 항법 알고리즘을 이용하여 자율 조작 실험을 성공적으로 수행한 결과는 제안된 항법 알고리즘의 높은 정확성과 유용 성을 동시에 나타낸다고 볼 수 있다.

4.4.2 수중 운동체의 작업 공간 자율 이동

수중 로봇의 작업 공간 이동을 자율로 수행한 결과를 Fig. 8에 보인다. 수중 로봇은 제어 패널의 중심으로부터 일정 반경으로 제어 패널의 반대편까지 이동한 뒤 같은 경로를 통해 초기 위치로 복귀하였다. 추정 항법은 센서의 표류 오차로 인해 시간이 지남에 따라 항법 정확도가 떨어지지만 제안된 항법 알고리즘은 참조 모델의 상대 위치 정보를 이 용하여 항법 오차를 보정하는 것을 확인할 수 있다.

Fig. 8

Snapshots and vehicle trajectories of the circling maneuver task around the subsea structure

5. 결 론

본 연구에서는 수중 구조물 주변에서 수중 로봇을 이용 자율 조작 임무 수행을 가정하고 이에 필요한 정확한 항법 정보를 제공하기 위해 모델 참조 수중 항법이 제안되었다. 모델 참조 수중 항법은 6자유도 운동 모델을 기반한 관성 항법에 더하여 컴퓨터 비전 기법인 모델 기반 포즈 추정 알고리즘으로부터 얻어진 수중 구조물과의 상대 포즈 정보 를 추가적인 항법 정보로 활용하여 관성 항법에서 발생하는 표류 오차를 보정한다.

모델 참조 수중 항법의 성능 및 유용성을 실험적으로 검 증하기 위해 제안된 항법 알고리즘을 수중 로봇에 적용하고 수중 구조물의 제어판 자율 조작과 작업 공간 자율 이동 실험이 수행되었다. 모든 실험 과정에서 제안된 항법 알고 리즘은 안정적이고 높은 정확도의 항법 성능을 보여주었을 뿐만 아니라 높은 실시간성 및 자율도를 보장하였다. 따라 서 실험을 통해 검증된 제안된 항법 기법의 성능과 유용성 을 감안할 때 추후 수중 구조물의 유지/보수 및 설치와 같은 다양한 수중 작업에 활용이 가능할 것으로 기대된다.

Acknowledgement

이 논문은 해양수산부 재원으로 한국해양과학기술진흥 원의 지원을 받아 수행된 연구(선체 수중검사 자동화를 위 한 자율무인시스템 개발 및 위험유해물질(HNS)사고 관리 기술 개발) 이다.

References

• G. Marani, S. Choi, and J. Yuh, “Underwater Autonomous Manipulation for Intervention Missions AUVs,” Ocean Engineering. Special Issue: AUV, vol.36, no.1, pp.15-23, 2009. [https://doi.org/10.1016/j.oceaneng.2008.08.007]
• P.J. Sanz, P. Ridao, G. Oliver, G. Casalino, Y. Petillot, C. Silvestre, C. Melchiorri, and A. Turetta, “TRIDENT: An european project targeted to increase the autonomy levels for underwater intervention missions,” in OCEANS’13 MTS/IEEE, 2013.
• N. Palomeras, A. Pnalver, M. Massot-Campos, G. Vallicrosa, P.L. Negre, J.J. Fernandez, P. Ridao, P.J. Sanz, G. Oliver-Codina, and A. Palomer, “I-AUV Docking and Intervention in a Subsea Panel,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, Chicago, USA, 2014. [https://doi.org/10.1109/IROS.2014.6942870]
• N. Palomeras, A. Carrera, N. Hurtós, G.C. Karras, C.P. Bechlioulis, M. Cashmore, D. Magazzeni, D. Long, M. Fox, K.J. Kyriakopoulos, P. Kormushev, J. Salvi, and M. Carreras, “Toward persistent autonomous intervention in a subsea panel,” Autonomous Robots
• A.I. Comport, E. Marchand, M. Pressigout, and F. Chaumette, “Real-Time Markerless Tracking for Augmented Reality: The Virtual Visual Servoing Framework,” IEEE Transactions on Visualization and Computer Graphic, vol.12, no.4, July/august, 2006. [https://doi.org/10.1109/TVCG.2006.78]
• P. Boutthemy, “A Maximum Likelihood Framework for Determining Moving Edges,” IEEE Transactions On Pattern Analysis And Machine Intelligence, vol.11. no.5, May, 1989. [https://doi.org/10.1109/34.24782]
• X. Gratal, C. Smith, M, Bjorkman, and D. Kragic, “Intergrating 3D Features and Virtual Visual Servoing for Hand-Eye and Humanoid Robot Pose Estimation,” IEEE-RAS Int. conf. on Humanoid Robotics, pp.240- 245, 2013. [https://doi.org/10.1109/HUMANOIDS.2013.7029982]
• B. Espiau, F. Chaumette, and P. Rives, “A New Approach to Visual Servoing in Robotics,” IEEE Transactions On Robotics And Automation, vol.8. no.3, June, 1992. [https://doi.org/10.1109/70.143350]
• E. Marchand, F. Spindler, and F. Chaumette, “ViSP for visual servoing: a generic software platform with a wide class of robot control skills,” IEEE Robotics and Automation Magazine, Special issue on “Software Packages for Vision- Based Control of Motion”, P. Oh, D. Burschka (Eds.), vol.12, no.4, pp.40-52, December, 2005. [https://doi.org/10.1109/MRA.2005.1577023]
• E. Rosten and T. Drummond, “Machine Learning for High Speed Corner Detection,” Proc. Ninth European Conf. Computer Vision, vol.1, pp.430-443, 2006. [https://doi.org/10.1007/11744023_34]
• E. Rublee, V. Rabaud, K. Konolige, and G. Bradski. “ORB: an efficient alternative to SIFT or SURF,” IEEE International Conference on Computer Vision, pp.2564-2571, 2011. [https://doi.org/10.1109/ICCV.2011.6126544]