선체 형상 정보를 활용한 3차원 위치인식 알고리즘 개발

2.1로봇기구학기반 3차원위치예측

본 절에서는 Fig. 1과 유사한 로봇의 바퀴 속도 및 선체 정보를 결합한 기구학 모델의 상태 변수를 정의한다. 해당 로봇은 SLM Global에서 제작된 선체 청소 로봇으로, 전방에 2개의 조향 바퀴를, 후방에 2개의 구동 바퀴를 가지고 있다. 이 로봇은 IMU, 엔코더, 압력 센서가 장착되어 있으며, 최대 60cm/s의 속도로 이동이 가능하다. 로봇의 상태는 바퀴의 Odometry와 선체형상 정보를 통해 정의된다. 선체는 Fig. 2와 같이 다양한 형상의 모델로 설계된다. 상기 로봇은 선체의 벽면을 따라 이동을 하며, 선체 상단에서 하단으로 이어지는 경로에 따라 기울기가 변화된다.

Fig. 1.

Wall cleaning robot built by SLM Global [10]

Fig. 2.

Ship shape models [11]

이는 선체의 설계 모델을 통해 확인할 수 있다. 우리는 선체 정보인 로봇의 이동거리(l_i)에 따른 각도(θ_i)정보와 로봇의 바퀴 속도를 통해 현재 위치를 예측하고자 한다. 로봇은 바퀴 속도를 측정하는 Encoder 데이터 값을 이용하여 현재 위치로부터 이동한 누적 거리를 계산할 수 있으며 재귀적인 방법을 사용한다. 수식 4는 바퀴 속도에 따른 이동 속도 및 각속도를 나타내며, 2차 Runge-Kutta(룬게-쿠타)를 통해 위치 값을 계산한다. 추가로 시뮬레이션의 신뢰성을 향상시키기 위해 Encoder에 가우시안 노이즈 (v_i)를 수식 5와 같이 설계하여 적용하였다.

수식 4와 Fig. 3에서 ω₁, ω_r는 2개의 바퀴 회전속도를, b, R은 바퀴 간의 거리와 바퀴의 반지름을 뜻한다. 이를 통해 로봇은 바퀴의 입력 값에 따른 이동속도, 및 각속도를 계산하고 현재 위치를 업데이트 한다. 수식 6에서 θ_{s, i}는 로봇의 Pitch 각도를 나타내며, 선체 형상 정보를 통해 도출된다. Fig. 4와 같은 선체 모델 도면의 x-z 평면 좌표를 추출하고 상단에서 하단으로 이어지는 경로(l_i)의 좌표 값을 활용하여 각도 값(θ_i)을 계산할 수 있다.

Fig. 3.

Robot state variables

Fig. 4.

Hull drawing information [12]

계산된 이동 경로에 따른 각도 값은 로봇 이동거리의 비를 통해 현재 위치에 따른 각도로 적용된다. 로봇의 3차원 위치는 수식 7-9에 의해 정의되며, 로봇의 현재 속도 및 각속도를 통해 위치를 계산한다. 이때 주행에 따른 로봇의 기울기 정보를 선체형상 정보 θ_{s, i}로 적용하며, 이전 스텝에서 계산된 결과를 누적함으로써, 현재 위치를 업데이트 한다.

위의 수식에서 사용된 아래 첨자 s는 로봇의 상태(State)를 뜻하며 Body coordinate을 따른다. 또한 p는 선박 도면에 위치한 point 좌표를 뜻한다.

2.2 센서 정보 기반 3차원 위치 추정

[13]에서는 로봇이 이동하는 지형 정보와 센서 정보를 융합한 2차원 위치 추정 알고리즘을 개발하였다. 상기 연구에서는 수중구조물의 외벽을 주행하는 로봇의 기구학적 관계와 센서 데이터를 결합하여 위치 추정 성능을 향상시킨 바 있다. 하지만 이는 평면 주행을 고려한 알고리즘으로 3차원 환경에서 활용하기에는 제약이 존재한다. 따라서 3차원 형상을 가진 선체에 적용 가능한 기구학적 관계를 도출하고자 한다. Fig. 5는 선체 형상 및 센서 데이터를 결합한 위치 추정 과정을 보여준다. 위치 추정은 x-z 평면과 y-z 평면에서 기구학적 관계를 이용하여 이루어지며, 압력센서 와 수심과의 관계식을 통해 z축의 위치를 측정한다. 센서 데이터에 따른 기구학적 관계를 통한 위치 추정은 아래의 수식을 통해 정의된다.

Fig. 5.

Position estimation method through kinematic relationship of wall cleaning robot

로봇의 실시간 각도 정보는 이전 데이터와 현재 데이터의 평균을 통해 수식 10과 같이 정의할 수 있다. 또한, Fig. 5에서 x-z 평면에서의 로봇 주행을 고려할 때, 로봇의 각도는 선체의 상단에서 하단으로 이어지는 Pitch 각도에 따라 변화한다. 이로 인해 수식 11과 같이 z 축의 변화율과 Pitch 각도의 변화율을 활용하여 로봇의 x 축 거리를 추정할 수 있다. 마찬가지로 수식 12와 같이 y-z 평면상 z 축의 변화율과 Yaw 각도의 변화율을 이용하여 y 축 거리를 추정할 수 있다. 하지만 이를 2차원 평면이 아닌 3차원으로 확장할 경우 문제가 발생한다.

x축 방향의 이동은 선체의 형상 특성으로 인해 z축 변화량과 각도 θ의 변화량이 서로 상관관계를 가지고 있어 문제가 없지만, Ψ의 변화량과 z축 변화량의 상관관계는 존재하지 않는다. 이로 인해 선체의 하부에서 미세한 z축 변화는 위치 추정에 오차를 발생할 수 있다. 따라서 로봇이 선체의 하부에 접근할수록 y축 이동의 추정은 y-z 평면에서 y-x 평면으로의 변화로 영향을 받게 된다. 이를 해결하기 위해 수식 12를 13과 같이 변형하여, y축 방향의 추정은 현재 각도 θ의 크기에 기반하여 결정되는 평면 비율을 사용한다.

2.3 파티클 필터 기반 데이터 융합

EKF(Extended Kalman Filter)는 비선형 시스템을 선형화하여 선형 시스템과 유사하게 다루는 알고리즘으로써, 데이터 융합에서 많이 사용된다. 하지만 상태 및 측정 방정식을 선형화하는 과정에서 오차가 발생할 수 있으며, 이로 인해 알고리즘이 제대로 작동하지 않을 수 있다. 또한, EKF는 센서 및 모델의 노이즈 분포가 가우시안을 가정하는데, 이는 실제 노이즈 모델과 차이가 있을 수 있다. 이와 대조적으로 파티클 필터는 샘플을 통해 다양한 형태의 분포를 추정할 수 있으며, 비선형 시스템을 선형화할 필요가 없어 고차원 및 복잡한 시스템에서 우수한 성능을 보인다[14]. 특히 [13]에서 제안한 로봇의 이동 경로는 특이점이 존재하며, 이는 선체 구조물 내에서 더 강조되는 문제로, 선체의 최하단부에서 θ=0일 때와 z_(e,i)-z_(e,i-1)=0일 때 문제가 발생한다. 이러한 불확실하고 급격한 변화로 인한 오차는 확장 칼만 필터(EKF)의 재귀적인 방식으로는 처리하기 어렵다. 하지만 파티클 필터의 경우 샘플들의 확률을 기반으로 하여 이러한 문제에 유연하게 대응할 수 있다. 아래의 수식은 파티클 필터를 통한 데이터 융합 과정을 소개한다[15].

위의 수식 13, 14의 x_k, z_k는 시스템 상태 x와 측정 값 z를 나타내며, 2.1, 2.2절에서 도출한 정보와 같다. 초기 파티클은 랜덤하게 위치시키며 본 논문에서는 1000개의 파티클을 사용한다. 이후 각 파티클의 위치를 예측하기 위해 아래의 수식 15와 같이 x_k의 정보를 사용한다.

수식 16에서 V는 시스템의 오차 랜덤계수이며, N은 선체정보에 대한 오차 랜덤계수이다. 각 파티클의 위치는 예측된 로봇의 상태에 오차 정보를 고려하여 지정된다. 파티클들의 가중치는 측정 값을 통해 계산되며 수식 17과 같다.

수식 17의 가중치가 적용된 파티클들은 정규화한 후 확률 분포로 변환하여 누적 분포 함수를 계산한다. 이를 통해 각 가중치 확률에 따른 파티클을 재 샘플링하고 평균하여 수식 18과 같이 로봇의 위치를 추정한다.

2.4 특이점 구간 보정 알고리즘

본 논문에서 다루는 로봇은 선체의 벽면을 따라 이동하는 특수한 환경을 가지고 있으며, 우리가 제안하는 위치 추정 알고리즘이 적용되지 않는 특이점이 존재한다. 따라서 본 절에서는 특이점 구간을 보정하기 위한 알고리즘을 제시하고자 한다. 특이점 구간은 선체의 최 하단부에 위치하며, 2.3절에서 언급한 바와 같이 z와 theta에대한 증가량이 0이 되어 추정 데이터의 업데이트가 불가능하다. 또한 특이점 주변 구간에서 역시 증가량이 점차 감소함에 따라 추정되는 위치 값에 불안정성이 발생한다. Fig. 6은 x축 샘플링에 따른 추정된 이동 증가량을 보여준다. Fig. 6에서 보듯이 데이터는 특이점 구간 주변에서 급격하게 성능이 하락 되는 모습을 보여준다. 상기 문제를 해결하기 위해 샘플링 되는 이동 증가량의 크기를 로봇의 입력에 따른 최대 속도와 비교하여 특이점을 감지하고 이전의 이동 증가량 값을 이용해 현재의 위치를 보정하도록 한다. 자세한 내용은 알고리즘 1과 같다. 알고리즘의 1번 줄에서는 추정된 로봇의 이동량이 로봇의 최대 이동량을 초과하는지를 확인한다. 그 다음, 2, 3번 줄에서는 현재 이동량을 이전 단계의 이동량으로 대체하고, 이를 x_offset변수를 통해 누적한다. 마지막으로, 5-8번 줄에서는 파티클 필터 알고리즘의 추정 데이터를 x_offset와 결합하여 특이한 구간에서 발생하는 오차를 보정한다. 이를 통해 현재 추정 값은 보정 데이터와 결합함으로써, 특이점에서의 오차를 해결할 수 있다.

Fig. 6.

Singularity section according to the movement path of the wall cleaning robot

Algorithm 1.

Data correction for outlier intervals

3.1 시뮬레이션 환경 구성

시뮬레이션 환경은 MATLAB을 통해 구축하였으며, 시뮬레이션의 신뢰성 검증을 위해 Encoder, IMU, 압력센서 데이터에 특정 노이즈를 포함하였다. Encoder의 노이즈는 10 dB의 크기를 가지는 백색 잡음으로 설정하였으며, IMU및 압력 센서에 의한 수심 노이즈는 각각 1deg, 0.05 m로 적용하였다. 시뮬레이션 내 로봇이 이동하는 선체 형상은 도면 정보를 통해 생성할 수 있으나, 정확한 알고리즘의 성능을 확인하기 위해 본 논문에서는 반지름 6 m를 가지는 반원의 형상으로 모델을 생성하였다.

3.2 시뮬레이션 환경 조건

3차원 기구학적 관계를 통한 위치 추정 알고리즘의 성능을 확인하기 위해 Fig. 7 (a)와 같이 로봇의 방향을 설정하여 시뮬레이션을 진행한다. 방향을 0deg로 설정할 경우 로봇은 x-z 2차원 평면 이동을 하며, 각도가 증가함에 따라 로봇은 3차원 평면으로 확장된다. 또한, 이동 경로 상 특이점 개수 ‘k’를 증가시켜 논문에서 제시하는 특이점 구간 보정 알고리즘의 성능을 확인하고자 한다. 각각의 조건은 Fig. 5와 같이 5개의 방향과 4개의 ‘k’ 값을 가지도록 한다.

Fig. 7.

Schematic diagram of simulation conditions. (a) determines the robot’s heading direction (b) determines the number of singular points on the robot’s movement path

3.3 시뮬레이션 결과

Figs. 8-11는 3차원 위치 추정을 위해 제시한 y축 위치 추정 방법에 대한 결과를 보여주며, Fig. 12-13은 특이점 개수 k에 따른 특이점 보정 알고리즘의 성능 결과를 보여준다. Fig. 8에서 추측항법(Dead Reckoning)의 결과는 2.1절에서 소개한 x_s,i의 데이터이며, Encoder 노이즈로 인해 실제 로봇 경로와 큰 차이를 보인 것을 확인할 수 있다. 또한, 2차원 위치 추정 방법은 초기에 높은 정확성을 보이지만, 선체의 하단부에 접근할수록 z축 변화에 따른 오차가 증가하는 것을 확인할 수 있다. 그에 반해, 제안한 3차원 위치 추정 방법은 선체 하단부 통과 과정에서도 성능 저하가 없음을 확인할 수 있었다. 이러한 결과가 뚜렷하게 드러나는 것은 Figs. 9, 10과 같으며, 2차원, 3차원 위치 추정 방법에 따른 로봇 상태 값 오차의 평균을 나타낸다. Fig. 9과 같이 2차원 위치 추정 방법은 로봇의 방향 각도가 증가함에 따라 하단부에서의y축 이동이 증가하여 오차가 증가하는 모습을 보인다. 반면 제시한 3차원 위치 추정 방법을 적용할 경우 하단부로 이어지는 Pitch 값에 의해 위치 추정을 하기 위한 평면이 변경되어 오차가 증가하지 않는 모습을 보인다. 이는 Fig. 11를 통해 확인할 수 있으며, 하단부에 가까워질수록 z축의 변화량이 감소하여 y축 이동량 추정이 정상적으로 적용되지 않는 것을 확인할 수 있다. 반면 3차원 추정 방법을 사용할 경우 이동 구간에 상관없이 로봇의 실시간 이동량 추정이 가능한 것을 보여준다. 하지만 Fig. 11에서 선체의 최 하단부 구간에서 특이점이 발생하는 것을 볼 수 있으며, 이동량 추정에 오차가 발생한다. 이를 해결하기 위한 특이점 보정 알고리즘을 적용한 결과는 Fig. 12과 같다. Fig. 12에서 보듯이 이동 경로 상 배치된 특이점으로 인해 알고리즘이 적용되지 않았을 때 오차가 누적되어 도착지점에서 큰 오차가 발생하는 것을 볼 수 있다. 반면 특이점 구간을 감지하고 오차를 보정하는 알고리즘 적용으로 인해 목표지점에서의 오차가 감소하는 것을 볼 수 있다. 추가로 지속적인 작업을 수행할 때 특이점 구간 증가에 따른 강인함을 확인하고자 Fig. 11과 같이 최대 128개의 특이점 구간을 생성하였다. 그 결과 알고리즘의 적용 여부에 따라 최대 0.47 m의 오차 차이가 보이는 것을 확인하였다. 하지만 알고리즘을 통해 모든 오차가 제거되지 않으며, 미세한 오차 누적으로 인해 특이점 128개의 구간에서 0.38 m의 오차를 보였다. 이를 해결하기 위한 특이점 보정 알고리즘을 적용한 결과는 Fig. 12과 같다. Fig. 12에서 보듯이 이동 경로 상 배치된 특이점으로 인해 알고리즘이 적용되지 않았을 때 오차가 누적되어 도착지점에서 큰 오차가 발생하는 것을 볼 수 있다. 반면 특이점 구간을 감지하고 오차를 보정하는 알고리즘 적용으로 인해 목표 지점에서의 오차가 감소하는 것을 볼 수 있다. 추가로 지속적인 작업을 수행할 때 특이점 구간 증가에 따른 강인함을 확인하고자 Fig. 11과 같이 최대 128개의 특이점 구간을 생성하였다. 그 결과 알고리즘의 적용 여부에 따라 최대 0.47 m의 오차 차이가 보이는 것을 확인하였다. 하지만 알고리즘을 통해 모든 오차가 제거되지 않으며, 미세한 오차 누적으로 인해 특이점 128개의 구간에서 0.38 m의 오차를 보였다.

Fig. 8.

Comparison of results according to location estimation method

Fig. 9.

2d estimation method error

Fig. 10.

3d estimation method error

Fig. 11.

Comparison of y-axis movement increment estimation results

Fig. 12.

Comparison of results according to outlier intervals

Fig. 13.

Comparison of results according to the number of outliers