레이저스케너 센서기반의 칼만필터 관측을 이용한 사람이동예측

Estimation of People Tracking by Kalman Filter based Observations from Laser Range Sensor

Abstract

For tracking a varying number of people using laser range finder, it is important to deal with appearance/disappearance of people due to various causes including occlusions. We propose a method for tracking people with automatic initialization by integrating observations from laser range finder. In our method, the problem of estimating 2D positions and orientations of multiple people's walking direction is formulated based on a mixture kalman filter. Proposal distributions of a kalman filter are constructed by using a mixture model that incorporates information from a laser range scanner. Our experimental results demonstrate the effectiveness and robustness of our method.

1. 서론

현대 생활에 있어 각종 보안 및 치안 지역내에서의 안전 불안감이 증가함으로 인하여 각종 감시 및 보안 장비들이 선보이고 있다. 그 중에서 휴먼인지로봇으로 대체한 인간의 의심행동을 사전에 방지하는 연구가 이뤄지고 있다. 이러한 시스템 구현을 위한 로봇의 기능으로 무엇보다 자율적인 인지 및 인식과 주행의 기능이다[1].

이동로봇이 자율적인 주행을 하기 위해서는 환경인식, 경로계획 및 주행제어, 그리고, 위치 인식 및 보정 등의 기본적인 능력을 갖추어야 한다[2]. 특히, 원활한 작업 수행을 위하여 임의의 경로로 자율적으로 이동하는 이동로봇의 경우에는 로봇 자신의 현재 위치를 추정하는 기능이 필수적으로 요구된다. 로봇이 자신의 위치를 추정하기 위한 방법으로는 로봇의 이동 바퀴에 회전수를 측정할 수 있는 엔코더를 장착하여 초기 위치로부터 이동거리를 더해 나감으로써 자신의 위치를 추정하거나 가속도 센서와 자이로 센서 등의 가속도 정보를 2차 적분하여 초기 위치로부터 변화된 자신의 위치를 알아내는 방법 등이 사용되어 왔다[3][4].

휴먼인지로봇의 우선적인 기능으로 자율적인 주행기능과 환경인식 기능을 우선적으로 들 수 있으나, 산업 환경에서 자율 이동 로봇(Autonomous Mobile Robot), 자율 운반체(Autonomous Guided Vehicle)등이 도입된 이래로, 유연한 탐색 주행 시스템(Flexible Navigation System)을 위한 요구로 이동 로봇이 원하는 위치로 이동하기 위해서는 경로 계획(Path Planning), 환경 인식(Environments Recognition), 장애물 회피(Obstacle Avoidance) 그리고 위치 추정(Position Estimation)등의 기능이 더욱 많이 요구된다. 실내에서 로봇을 이용한 경비 기능으로는 의심스러운 행동을 취하는 인물에 접근하고 주의를 촉구 같은 것이 있지만, 이러한 업무는 기존의 시스템에서는 대체 수 없다[5]. 따라서 본 연구에서는 경비 로봇에 주목하고 “이동성”이라는 장점을 보다 적극적으로 활용함으로써 로봇의 동작에 의한 주의환기, 인간의 행동 제어 기반의 로봇 시스템의 구축을 목적으로 한 인간 위치인식과 로봇의 위치제어의 연구결과를 제시하였다.

2. LRF기반 행동검출

2.1 정지 물체 영역의 추정

실내 환경에 이동로봇에 장착된 레이저 파인더 센서에 의한 상대 위치와 이동로봇의 위치 데이터와 통합하여 세계 좌표계에 여러 스캐닝 데이터가 얻어진다. 여기에서는 이러한 스캐닝 데이터를 이용하여 인물 추적 방법을 기술하고자 한다. 데이터의 통합에 의해 얻은 스캐닝 데이터를 정지한 사람과 이동 중인 사람을 구별하기 위해 정지 물체 영역에 대한 추정을 실시한다.

정지 물체 영역에 들어 있지 않은 데이터에서 다음과 같이 이동 중인 사람의 다리를 추출 할 수 있다. 그림 1은 보행시 인간의 발목 부위를 센서로 센싱 할 때 획득하는 센싱데이터의 모델화로 나타내었다.

(1) 정지 물체 영역에 들어 있지 않은 스캐닝 데이터 가운데 특정거리 이하로 이어져있는 스캐닝 데이터를 하나의 집합으로 본다.

(2) 스캐닝 데이터 집합의 양끝 파티클 사이의 거리를 다리의 폭 w/2로 규정하였다.

(3) 스캐닝 데이터 집합의 중간점에서 LRF에서 멀어지는 방향으로 w/2 떨어진 점을 다리의 중심으로 간주 하였다.

Fig. 1 Estimation of the leg position

2.2 이동궤적추출

이동 궤적을 추출하기 위한 변수로는 위치 p^k_t = (x^k_t, y^k_t), 이동속도 v^k_t = (vx^k_t, vy^k_t),이동방향 d^k_t = arctan(x^k_t - x^k_t-1, y^k_t - y^k_t-1)로 정하였고, 가장 기본적인 이동에 따른 물리량으로 표시할 수 있다. 멈춘 시간 T는 일정 시간 동안 움직이지 않고 정지된 시간을 나타낸다. 경로의 유연성, ⍺는 이동 방향의 유사성 θ^k_t를 사용하여 식 (1)∼(3)으로 나타낼 수 있다.

\(\begin{align}\theta_{t}^{k}=\arccos \frac{d_{t-1}^{k} \cdot d_{t}^{k}}{\left\|d_{t-1}^{k}\right\|\left\|d_{t}^{k}\right\|}\end{align}\)       (1)

\(\begin{align}\overline{\theta_{t}^{k}}=\frac{1}{2}\left(\theta_{t-1}^{k}+\theta_{t}^{k}\right)\end{align}\)       (2)

\(\begin{align}\alpha_{t}^{k}=\sqrt{\frac{1}{2} \sum_{i=t-1}^{t}\left(\theta_{i}^{k}-\overline{\theta_{t}^{k}}\right)^{2}}\end{align}\)      (3)

본 실험에서는 스캐닝 데이터의 오차에 따른 추적 결과는 유연한 궤적 값으로 도출되지 않지만, 취득한 데이터에 대한 평활화 필터링 과정을 거친 후 유연한 궤적 특성을 도출할 수 있다. 또한 복수의 이동 중인 사람의 이동 특성중 중요한 보행 특징정보는 보행 속도, 활보의 길이(좌우 보폭을 포함한 길이), 왼쪽 다리의 스텝과 오른쪽 다리의 스텝 사이의 소요시간을 나타내는 케이던스(보수/분)로 나타낼 수 있다 [8]. 관찰자의 육안 판정이 용이하기 때문에 케이던스가 잘 사용되고 있는 것이지만 측정시간이 길기 때문에 동작 인식에는 적합하지 않다. 따라서 본 실험 방법에서는 직관적이고 분절화가 가능하도록 이동자의 특유의 보행 주기 w^(L/R)(초 /한보)를 적용하였다.

보행중인 좌우 보폭의 차이를 명확하게 관찰하기 위하여 활보의 길이정보 보다는 보폭 s^(L/R) 정보를 이용하였다. 보행은 입각기(stance phase: 발이 지면에 닿아 있는 시기로서 정상 보행의 60%를 차지함)와 유각기(wing phase: 발이 지면에서 떨어져 있는 시간, 정상 보행의 40%를 차지함)에 나누어 적용하였고 그 비율은 r^(L/R)로 하였다.

3. 보행자의 이동궤적 추종

본 절에서는 레이저스케너의 이동물체 추적을 위한 이동물체의 운동을 분석 및 모델화하여 운동 성분을 추정하여 물체의 거동 및 궤적을 예측하는 내용에 대하여 다루었다. 본 연구에서는 이동물체의 운동모델로부터 칼만필터를 사용하여 이동물체의 상태를 추정하였으며 이로부터 최소제곱 추정법(LSE: Least-Squares Estimator)을 사용하여 이동물체의 선형속도와 회전각속도를 추정하여 물체의 궤적을 예측하는 방법을 사용하였다[6].

3.1 이동물체의 운동모델

직교좌표 공간상에서 운동하는 일반적인 물체에 대하여 각 방향의 속도 및 가속도를 추정할 수 있으면 δt 이후의 공간상에서 예측되는 물체의 x, y, z 각 방향의 위치는 식 (4)와 같다[9].

\(\begin{align}\begin{array}{l}\hat{P}_{x+\delta t}=\hat{P}_{x}+\hat{V}_{x} \delta t+\frac{1}{2} \hat{A}_{x} \delta t^{2} \\ \hat{P}_{y+\delta t}=\hat{P}_{y}+\hat{V}_{y} \delta t+\frac{1}{2} \hat{A}_{y} \delta t^{2} \\ \hat{P}_{z+\delta t}=\hat{P}_{z}+\hat{V}_{z} \delta t+\frac{1}{2} \hat{A}_{z} \delta t^{2}\end{array}\end{align}\)       (4)

x-y평면상에서 이동하는 물체의 움직임은 진행속도 성분 v_k과 각속도 성분 w_k로 분해하여 식 (5)의 이산형 시변 방정식으로 나타낼 수 있다[9]. 이동물체의 운동을 일으키는 v_k와 w_k의 변화는 식 (5)와 같이 랜덤한 운동으로 정의한다. 여기서 ξ_v, ξ_w는 가우시안 랜덤변수이다.

\(\begin{align}\begin{array}{l} \delta x_{k+\delta, t k}=v_{k} \cos \left(\theta_{k}\right) \delta t-\frac{1}{2} \omega_{k} v_{k} \sin \left(\theta_{k}\right) \delta t^{2} \\ \delta y_{k+\delta, k}=v_{k} \sin \left(\theta_{k}\right) \delta t+\frac{1}{2} \omega_{k} v_{k} \cos \left(\theta_{k}\right) \delta t^{2} \\ \delta \theta_{k+\delta, k}=\omega_{k} \delta t \\ \delta v_{k+\delta, k}=\xi_{v}, \delta \omega_{k+\otimes, k}=\xi_{\omega} \end{array}\end{align}\)     (5)

이동물체의 모델을 칼만필터에 적용하기 위해서는 식 (6)의 이산시간 상태천이 모델이 필요하다. 이동물체의 이산형 시변 방정식으로 부터 이동물체 상태 천이 모델과 관측 모델을 얻는다. 여기서 z는 측정벡터, H는 상태벡터와 측정벡터사이의 관측행렬이며 w는 상태천이 자체의 불규칙 성분이며 v는 측정잡음, T는 샘플링 시간을 의미한다.

x_k = Φ_k,k-1x_k-1 + w_k-1

z_k = H_kx_k + v_k      (6)

3.2 칼만필터에 의한 이동물체의 상태추정

보행자의 양 다리에서 추출한 스캐닝정보에서 동일 보폭 정보를 기반으로 동일인으로 간주할 수 있도록 칼만 필터를 이용하여 인물의 추적을 실시하였다. 우선 동일 인물의 다리 쌍은 (1) 일정한 거리 이상 떠나지 않는다. (2) 진행 방향이 거의 같다는 특징을 가진다. 이러한 조건을 일정 기간 충족하는 다리의 쌍을 동일 인물의 다리 쌍으로 간주하였다. 이와 같이 다리의 위치를 구한 후 그 중간 지점을 인물 위치로 결정할 수 있다.

동일인으로 간주된 다리의 쌍은 칼만 필터에 의해 이동궤적에 대한 연장 처리가 적용된다. 다음 프레임에서 궤적이 연장된 각각의 다리는 관측된 다리 후보 중에서 가장 가까운 것을 선택하고 그 프레임을 현 다리의 이동 위치로 결정한다. 이것을 반복하여 추적을 구현하도록 하였다. 여기서 적용한 칼만 필터는 다리의 가속도를 외부 입력으로 확장하였고 다리의 가속도는 기 연구된 보행 모델[7]을 이용하여 적용하였다. 적용된 보행 모델은 4개의 위상으로 나누어져 있으며 과거의 데이터에서 얻은 다리의 속도 및 가속도에서 현재의 위상을 결정할 수 있다. 그리고 결정된 위상에 따라 가속도를 구할 수 있다.

이동 중인 보행자의 보폭 및 속도 등의 스케닝 입력정보는 처리과정 가운데 불확실성 및 잡음을 포함하고 있으며 상태천이 자체도 불규칙한 성분이 존재하므로 이에 대해 강인하게 상태를 추정하는 관측기로는 칼만필터가 적합하다. 칼만필터는 추정값과 측정값의 오차에 필터이득을 곱하여 상태천이 모델에 수정함으로써 추정오차를 최소화하는 처리과정이다. 이동물체의 상태추정 문제에서 칼만필터의 측정벡터는 이미지에서 구한 이동물체의 x, y 평면공간상 위치이며, 이로부터 상태변수로 설정한 이동물체의 x, y 위치, 방향, 진행방향속도, 회전각속도를 추정한다[8].

그림 2는 칼만필터의 처리과정을 나타낸 것이다. 필터이득을 구하기 위하여 추정오차의 공분산행렬이 필요하며 이는 식 (7)로 이전과정의 공분산행렬의 상태 천이에 시스템잡음의 공분산 행렬 Q_k-1가 부가된 형태로 추정된다.

P'_k = Φ_k,k-1P_k-1Φ^T_k,k-1Q_k-1       (7)

Fig. 2 Kalman filter processing

추정오차의 공분산 행렬 P'_k와 관측 행렬 H_k, 측정잡음의 공분산 행렬 R_k로 추정오차를 최소로 하는 최적의 필터이득을 구하며 이는 식 (8)의 형태이다.

K_k = P'_kH^T_k{H_kP'_kH^T_k + R_k]^-1       (8)

다음은 상태를 추정하는 과정으로 식 (9)와 같이 추정벡터의 상태천이 모델에 이전단계의 추정벡터와 측정벡터 z_k간의 오차에 필터이득을 곱한 이노베이션(innovation)항이 부가되어 최적의 상태를 추정한다.

\(\begin{align}\hat{\mathbf{x}}_{k}=\Phi_{k, k-1} \hat{\mathbf{x}}_{k-1}+K_{k}\left[z_{k}-H_{k} \Phi_{k, k-1} \hat{\mathbf{x}}_{k-1}\right]\end{align}\)       (9)

최종적으로 식 (10)의 추정오차의 공분산 행렬을 수정하는 과정을 거치고 다시 식 (8)의 과정으로 순환한다.

P_k = P'_k - K_kH_kP'_k       (10)

4. 환경인식 로봇시스템

실험을 위해 사용한 로봇과 센서시스템은 그림 3에서 제시한 것과 같이 Hokuyo Automatic 사의 소형⋅고성능의 스케너식 레인지 센서(UBG04-LX)로서 거리를 측정하는 센서로서 장애물 회피 및 지도를 작성하는데 사용하였다[5]. 소형⋅경량이면서 측정거리는 20mm에서 5,600mm까지의 범위에서 정확한 측정이 가능하다. 동작범위로는 240°(-30°∼ 210°) 이고 각 스텝마다(총 683스텝) 약 0.36°의 분해능을 갖고 있다. 또한 응답주기가 약 38Hz로 빠르며, 이동하면서 인간을 측정하는데 적합한 측역센서라고 할 수 있다.

Fig. 3 Mobile robot for experiment

레이저스케너의 동작은 호스트에서 레이저파인더로 정해진 커맨드를 전송하면 이에 해당되는 패킷을 다시 호스트로 전송하는 방식으로 이루어진다. 측정 데이터를 얻기 위해서 레이저 레인지파인더에서 MDMS라는 명령 메시지를 전송해야 하는데 이 명령 메시지 안에는 레이저파인더의 스캔 시작 스텝과 끝 스텝 값을 포함하고 있다. URG-04LX 정면을 90°로 보았을 때 데드존을 제외한 측정 가능한 범위는 스텝44(-30°)를 시작으로 스텝 725(210°)까지 가능하다. 그림 4는 로봇에 적용된 레이저파인더의 외형을 보여주고 있다.

Fig. 4 Experimental Results

5. 실험

본 실험에서는 보안 및 공공건물의 실내에서 출입하는 외부인이나 다수의 보행자의 행동을 제한하거나 감지하여 해당 타깃 고객에 대한 지속적인 안내서비스나 마케팅을 위한 활용도를 높이기 위하여 타깃 고객에 대한 지속적인 추적 능력을 검증하기 위하여 아래와 같은 실험을 실시하였다. 우선 휴먼인지로봇이 주행하면서 위치 추정을 위한 방법으로 보행자의 움직임을 이용한 위치추정 방법을 제시하였다.

연구에서 제시한 방법으로 타깃이 되는 한 명의 사람을 이동 중인 사람으로 지정하고 발목 부분에 대한 센싱 데이터를 지속적으로 획득 및 학습시킴으로 해서 로봇이 추적할 수 있도록 실험을 수행하였다. 사람의 위치를 관측하는 벡터에 분산 1의 가우시안 백색 잡음을 인가하여 관측시 불확실성을 가지게 하였으며 이동 중인 사람의 입력에도 잡음을 인가하여 불규칙 운동을 하도록 설정하였다.

실험을 위한 보행자의 초기위치는 (50, 50), 휴먼인지 로봇의 위치는 (-50,-50)로 하여 70cm 떨어진 뒤에서 추적하도록 설정하였다. 유먼인지 로봇의 최고속도는 30cm/sec로 제한 하였고 보행자는 로봇보다 빠른 이동이 가능하도록 설정 하였다. 그림 4는 이동 중인 사람의 이동궤적에 대한 속도 모델을 식 (11)과 같은 형태로 주었을 때의 실험한 결과를 보여주고 있다. 여기서 진행방향은 분산 2, 회전각속도는 분산 0.1의 가우시안 랜덤 변수이다.

v(k) = 20(cos(0.03k)+1) + ξ_v [cm/sec]

\(\begin{align}\omega(k)=0.7 \sin \left(0.04 k+\frac{\pi}{1.5}\right)+\xi_{\omega}[\mathrm{rad} / \mathrm{sec}]\end{align}\)       (11)

그림 4(a)는 이동 중인 사람의 궤적과 로봇의 궤적, 예측한 궤적을 나타낸 것으로서 이동 중인 사람이 방향을 전환할 때 더 빨리 포획할 수 있는 방향으로 로봇이 이동했음을 확인하였다. 그림 4(b)는 이동 중인 사람과 로봇의 거리, 이동 중인 사람의 실제 진행속도와 칼만필터로 추정한 진행속도, 그리고 실제 회전 각속도와 추정한 각속도를 나타낸다. 처음에는 추정한 정보에 오차가 크게 나타나지만 이는 곧 수렴되며 실제값에 근사하게 상태가 추정됨을 알 수 있다.

6. 결론

본 논문에서는 공공건물 및 보안지역의 내에서 인간의 출입에 따른 서비스 제공이나 출입제한 안내를 위한 방법으로 안내서비스 역할을 수행할 수 있는 휴먼인지로봇의 추적 동작을 수행하기 위한 보폭기간의 동일인의 패턴을 기반으로 한 알고리즘을 제시하였다. 보행자의 보폭을 기반으로 행동을 인식하고 동일인으로서의 추종이 가능하도록 칼만필터 기반의 추적기능을 제시하였고 실험을 통하여 보행 보폭기반의 로봇의 추적 수행할 수 있는 실험결과를 통한 제안알고리즘의 유효성을 검증하였다. 구체적인 실험으로써 휴먼인지로봇이 이동하는 인간을 추적하고 차단하기 위한 방법을 다루었으며 실험을 통하여 그 유효성을 검증하였다. 제시한 방법은 크게 다음으로 구분된다.

⋅레이저스케너 기반 인간과의 거리 및 방향벡터 관계를 고려한 인간의 위치 추정 알고리즘 적용

⋅이동 인간의 운동을 칼만필터를 이용한 상태추정과 이후의 움직임을 예측하는 방법

⋅예측된 궤적에서 최단시간내에 인간의 움직임을 차단하기 위한 로봇 동작계획 방법

향후 추가적인 연구과제로는 레이저 파인더 센서 기반의 복수의 보행자 인식에 대한 강인성 문제에 관한 연구가 계속되어야 할 것이다.