KR20050044967A

KR20050044967A - 이동물체의 진행방향 추정 방법 및 시스템

Info

Publication number: KR20050044967A
Application number: KR1020030078873A
Authority: KR
Inventors: 권웅; 노경식; 한우섭; 심영보; 세르게이볼디레브
Original assignee: 삼성전자주식회사
Priority date: 2003-11-08
Filing date: 2003-11-08
Publication date: 2005-05-16
Also published as: JP4170284B2; CN1651862A; KR100580628B1; CN100562711C; EP1530024A1; US7543392B2; US20050125108A1; JP2005140789A

Abstract

본 발명은 이동물체의 진행방향 추정 방법 및 시스템에 관한 것으로서, 그 방법은 이동물체의 콤파스 정보로부터 자기장을 구하는 단계; 이동물체의 자기장을 소정의 값과 비교하여 그 결과에 따라 이동물체가 존재하는 지점이 특정 영역에 속했는지 여부를 판단하는 단계; 및 판단 결과에 따라 콤파스의 방위각을 이동물체의 방향추정에 이용할지의 여부를 결정하여 이동물체의 방향을 추정하는 단계를 포함함을 특징으로 하고, 그 시스템은 자이로, 오도미터 및 콤파스가 설치된 이동물체에 있어서 상기 콤파스가 설치된 이동물체에 인가된 자기장의 크기를 계산하는 자기장산출부; 자기장의 크기와 지구자기장의 크기의 차를 구하여 소정의 값과 비교하는 자기장비교부; 비교 결과에 따라 이동물체가 존재하는 지점이 지구자기가 우세하게 작용하는 영역에 속했는지 여부를 판단하는 지구자기영역 판단부; 및 판단 결과에 따라 콤파스의 방위각을 이동물체의 방향추정에 이용할지를 결정하여 이동물체의 방향을 추정하는 진행방향추정부를 포함함을 특징으로 한다.

본 발명에 의하면, 자이로, 콤파스 및 오도미터 센서의 단점을 극복하고 이동물체의 이동시 발생할 수 있는 돌발상황에 대한 대처가 가능하고, 다양한 종류의 에러원을 추정할 수 있으며, 시스템 폴트(System Fault)의 감지도 할 수 있다.

Description

이동물체의 진행방향 추정 방법 및 시스템{A heading direction estimation method and system for a mobile body}

본 발명은 이동물체의 위치 추정에 관한 것으로서, 특히 콤파스(compass) 데이터를 이용한 이동물체의 진행방향(heading direction) 추정 방법 및 시스템에 관한 것이다.

이동물체의 자세추정은 절대센서 또는 상대센서를 이용하여 물체의 위치 및 방향을 추정하는 문제로서, 도 1에 도시된 바와 같이 2차원 평면에서 움직이는 이동로봇의 경우에는 (x,y) 위치 및 방향 θ를 추정하는 문제이다. 절대센서는 로봇 또는 물체의 상대적인 움직임이 아닌 절대적인 위치 또는 자세를 측정할 수 있는 센서로서 카메라(Camera), 레이저 스캐너(Laser Scanner), 소나(Sonar), 위성 위치확인 시스템(GPS), 콤파스(Compass) 등이 있다. 이에 반해 상대센서는 로봇 또는 물체의 상대적인 변량을 측정하고 이를 통합함으로써 위치 또는 자세를 측정할 수 있는 센서로서 자이로(Gyro), 가속도계, 오도미터(odometer, 모터에 부착된 엔코더(encoder)) 등이 이에 해당된다.

로봇의 절대센서 중 카메라는 주변환경의 조명상태에 민감하고, 불확실한 정보의 데이터를 줄 가능성이 높으며, 레이저 스캐너는 정확한 데이터의 획득이 가능하나 가격이 매우 비싸고, 장애물이 많을 경우 측정하기 어렵다는 단점이 있다. 또한 소나는 데이터의 정확성이 떨어지며, GPS는 실외에서만 사용이 가능한데다 정밀도가 떨어지고, 콤파스는 실내에 혼재하는 외란 자기장의 영향을 받을 가능성이 높다는 단점이 있다. 한편 자이로, 오도미터, 가속도계 등의 상대센서는 변화량만을 측정하므로 이를 적분하는데서 발생하는 적분오차를 피할 수 없고, 자이로, 가속도계에 고유하게 존재하는 드리프트오차를 회피할 수 없다. 본 발명은 절대센서 중 콤파스를 이용하고, 상대센서 중 자이로와 오도미터를 이용하면서 동시에 센서퓨전을 구현하여 이러한 단점들을 보완하고자 한다.

기존의 추측항법(dead-reckoning) 자세 추정 방식은 사용된 센서에 따라 오도미터(Odometer)만을 사용한 방식, 자이로와 오도미터를 사용한 방식, 콤파스와 오도미터를 사용한 방식으로 구분된다. 오도미터 만을 사용한 방식은 가장 간단한 방식이지만, 미끄러짐 에러(slippage error), 범프 충돌(bump collision), 공중이동(kidnapping) 문제 등에 대처할 수 없고 에러가 계속 누적되므로 한계가 없는 에러(unbounded error)를 가진다.

이를 보완하기 위하여 오도미터에 자이로를 추가하여 추측항법(dead-reckoning)을 수행한 방식이 개발되었다. 그러나 이 방식은 오도미터 즉 모터 인코더(Encoder)만을 단독으로 사용하는 방식에 비해 정확한 결과를 얻을 수 있지만, 인코더와 자이로 모두 상대 센서이므로 장기적으로는 역시 에러의 한계(boundedness)를 보장할 수 없다. 자이로 대신에 절대 센서인 콤파스를 도입하여 장기적으로는 진행방향(Heading direction)을 안정적으로 검출할 수 있는 방식도 개발되었으나, 가정환경, 사무실환경 등에 항상 존재하고 있는 외란 자기장의 영향을 받기 쉬우므로 이 방식 그대로 실용화되기는 어렵다.

최근 들어, 자이로, 콤파스, 오도미터 3개 모두를 이용하는 방식도 제안되고 있다. 이는 자이로와 콤파스를 동시에 사용하므로 상호보완이 가능한 장점을 가지고 있으나, 현재 개발된 대부분의 방식은 통계에 기반한 센서 퓨전(Statistically based Sensor Fusion) 방법인 기존의 칼만필터(Kalman Filter)를 그대로 사용하므로써 여러 가지 한계를 가지고 있다. 즉 시스템 잡음(System noise) 및 측정 잡음(measurement noise)이 가우시안 백색 잡음(Gaussian white noise) 특성을 가지고 있어야 하므로 자기장, 장애물, 미끄러운 바닥 등 각종 불확실한 상황에 대처하기 힘들다. 그 외에도 센서의 에러 모델에 대한 정확한 정보 및 에러소스(error source) 간에 서로 상관되어 있지 않아야(uncorrelated) 한다는 가정이 필요하나 이는 현실적으로 구현이 불가능하다. 또한 센서의 특성이 센서 퓨전의 성능에 중요한 요소로 작용하므로 이 방식은 대부분 고가의 센서를 사용하고 있다.

본 발명이 이루고자 하는 기술적 과제는 이동물체의 진행방향 각도를 정확하고 견실하게 추정하여 물체의 자세정보를 산출할 수 있는 센서 퓨전을 이용한 이동물체의 진행방향 추정 방법 및 시스템을 제공하는 것이다.

본 발명이 이루고자 하는 다른 기술적 과제는 상기의 이동물체 진행방향 추정 방법을 컴퓨터에서 실행시키기 위한 프로그램을 기록한 컴퓨터로 읽을 수 있는 기록 매체를 제공하는 것이다.

상기 기술적 과제를 이루기 위한 본 발명에 의한 이동물체의 진행방향 추정 방법은, 이동물체의 콤파스 정보로부터 자기장 정보를 구하는 제1단계; 상기 이동물체의 자기장을 소정의 값과 비교하여, 그 결과에 따라 이동물체가 존재하는 지점이 특정 영역에 속했는지 여부를 판단하는 제2단계; 및 상기 판단 결과에 따라 콤파스의 방위각을 이동물체의 방향추정에 이용할지의 여부를 결정하여 이동물체의 방향을 추정하는 제3단계를 포함함을 특징으로 한다.

상기 소정의 값은 지구자기장의 크기이고, 상기 특정 영역은 지구자기가 우세하게 작용하는 영역임이 바람직하다. 상기 제2단계는 상기 이동물체에 두 개의 콤파스가 설치되어 있을 경우, 이동물체의 첫번째 콤파스의 자기장 크기 및 두번째 콤파스 자기장 크기를 각각 상기 지구자기장 크기와 비교하여 그 차가 둘 다 소정의 한계 값 보다 작은 경우, 하나만 소정의 한계 값보다 작은 경우, 둘 다 소정의 한계 값보다 작지 않은 경우로 나누어서 그 결과에 따라 이동물체가 존재하는 지점이 특정 영역에 속했는지 여부를 판단함이 바람직하다.

상기 제3단계는 이동물체가 특정영역에 속하여 콤파스의 정보가 유효하다고 판단할 경우 콤파스의 최종방위각을 구하고 상기 방위각을 측정입력으로 채택한 칼만필터를 이용하여 최적의 진행방향각도를 추정하고, 이동물체가 특정영역에 속하지 아니하여 콤파스의 정보가 유효하지 않다고 판단할 경우 자이로와 오도미터의 각속도를 비교하여, 그 차이가 소정의 값보다 작을 경우 오도미터로부터 구한 방향을 측정입력으로 채택한 칼만필터를 이용하여 최적의 진행방향을 추정하거나, 상기 차이가 소정의 값보다 작지 않을 경우 자이로의 각속도를 적분하여 진행방향을 추정하는 단계임이 바람직하다.

상기 이동물체의 첫번째 콤파스 자기장 크기 및 두번째 콤파스 자기장 크기를 각각 상기 지구자기장 크기와 비교하여 그 차가 둘 다 소정의 한계 값 보다 작은 경우의 이동물체의 방향추정은 상기 첫 번째 콤파스의 방위각과 두 번째 콤파스의 방위각의 차를 구하는 단계; 상기 방위각의 차가 소정의 값보다 작은 경우에는, 지구자기장과 콤파스들의 자기장의 차에 따라 가중치를 달리하여 이동물체의 콤파스 방위각을 구하는 가중치단계; 및 상기 방위각의 차가 소정의 값보다 작지 않은 경우에는, 이동물체에 대한 자이로의 각속도와 콤파스의 방위각에 대한 각속도의 차를 각각 구해 그 크기에 따라 이동물체의 콤파스 방위각을 구하는 각속도단계를 포함함을 특징으로 한다.

상기 가중치 단계는 , 을 샘플링 주기 동안 콤파스 1 및 콤파스 2의 방위각 변화량, 는 자이로의 각속도, 을 샘플링 시간이라 하고, 상기 첫 번째 콤파스의 자기장크기와 지구자기장 크기의 차를 , 상기 두 번째 콤파스의 자기장크기와 지구자기장 크기의 차를 라 할 때, 상기 와 의 곱을 구해 그 값이 음수이면, 수학식 3에 의해 구해지는 값을 콤파스의 최종 방위각()로 하고, 만일 상기 와 상기 의 곱이 양수이면, 수학식 4에 의해 구해지는 값을 최종 방위각( )로 하고, 만일 상기 곱의 값이 0 이면, 수학식 5에 의해 구해지는 값을 콤파스의 최종 방위각()로 함이 바람직하다. 상기 각속도 단계는 , 을 샘플링 주기 동안 콤파스 1 및 콤파스 2의 방위각 변화량, 는 자이로의 각속도, 을 샘플링 시간이라 할 때, 상기 첫번째 콤파스의 방위각()과 두번째 콤파스의 방위각( )의 차가 소정의 값보다 작지 않은 경우에는 수학식 6을 만족하는지를 체크하는 단계; 상기 수학식 6을 만족하면, 콤파스1의 방위각()을 콤파스의 최종 방위각( )로 하는 단계; 만일 수학식 6을 만족하지 않고 수학식 7을 만족하면, 콤파스2의 방위각( )를 콤파스의 최종 방위각( )로 하는 단계; 만일 수학식 6과 수학식 7에서 양변이 서로 같으면 상기 수학식 5에 의해 구해지는 값을 이동물체의 진행방향각도( )로 함이 바람직하다.

[수학식 3]

[수학식 4]

[수학식 5]

[수학식 6]

[수학식 7]

상기 이동물체의 첫번째 콤파스 자기장 크기 및 두번째 콤파스 자기장 크기를 각각 상기 지구자기장 크기와 비교하여 그 차 중 하나만 소정의 한계값 보다 작은 경우의 이동물체의 방향추정은 상기 한계값보다 작은 콤파스에 대한 방위각을 콤파스의 최종 방위각으로 함이 바람직하다. 상기 이동물체의 첫번째 콤파스 자기장 크기 및 두번째 콤파스 자기장 크기를 각각 상기 지구자기장 크기와 비교하여 그 차가 둘 다 소정의 한계값 보다 작지 않은 경우의 이동물체의 방향추정은 콤파스 데이터를 사용하지 않고 이동물체의 휠속도에 대한 각속도(즉 오도미터에 의해 구해진 각속도)를 계산하는 단계; 상기 이동물체의 자이로 각속도와 상기 휠의 각속도의 차를 구하는 단계; 상기 자이로 각속도와 휠 각속도의 차가 소정의 값보다 작으면, 오도미터로부터 구한 방향을 측정입력으로 채택한 칼만필터를 이용하여 최적의 진행방향각도를 추정하는 단계; 및 상기 자이로 각속도와 휠 각속도의 차가 소정의 값보다 작지 않으면, 칼만필터를 사용하지 않고 자이로의 각속도를 적분하여 진행방향각도를 추정하는 적분추정단계를 포함함이 바람직하다. 상기 적분추정단계는 소정 시간 내에 상기 적분추정단계가 소정 횟수 이상 수행될 경우에는 시스템을 종료하는 단계를 더 구비함을 특징으로 한다. 상기 이동물체의 진행방향 추정방법은 에러 상태 피드백 칼만 필터를 통해 이동물체의 진행방향 각도를 필터링하여 이동물체에 대한 최적의 진행방향 추정치를 계산하는 단계를 더 구비함을 특징으로 한다.

상기 기술적 과제를 이루기 위한 본 발명에 의한 이동물체의 진행방향 추정 시스템은, 자이로, 오도미터 및 콤파스가 설치된 이동물체에 있어서 상기 콤파스가 설치된 이동물체의 자기장의 크기를 계산하는 자기장산출부; 상기 자기장의 크기와 지구자기장의 크기의 차를 구하여 소정의 값과 비교하는 자기장비교부; 상기 비교 결과에 따라 이동물체가 존재하는 지점이 지구자기가 작용하는 영역에 속했는지 여부를 판단하는 지구자기영역 판단부; 및 상기 판단 결과에 따라 콤파스의 방위각을 이동물체의 방향추정에 이용할지를 결정하여 이동물체의 방향을 추정하는 진행방향추정부를 포함함을 특징으로 한다.

그리고 상기 기재된 발명을 컴퓨터에서 실행시키기 위한 프로그램을 기록한 컴퓨터로 읽을 수 있는 기록매체를 제공한다.

이하, 첨부된 도면들을 참조하여 본 발명에 따른 방법 및 장치에 대해 상세히 설명한다. 물체의 롤(Roll)과 피치(Pitch)는 가속도계 및 자이로를 이용하여 정확하게 계산할 수 있으나, 물체의 요(Yaw)값을 구하는 것은 매우 어려운 문제로 알려져 있다. 본 발명에서는 이동물체의 진행방향을 정확하고 견실하게(robust) 추정하여 물체의 자세 정보를 산출할 수 있는 새로운 센서 퓨전(Sensor Fusion)을 이용하여 이동물체의 진행방향을 추정한다. 즉 콤파스, 자이로, 오도미터 등의 절대센서와 상대센서를 조합하여 자기장, 장애물, 미끄러운 바닥 등 각종 불확실한 환경에 효과적으로 대처하면서 이동물체의 요 각도(Yaw Angle)를 획득하여 위치 및 자세를 최적으로 추정한다.

도 2는 본 발명에 의한 이동물체의 진행방향 추정을 위한 이동물체에 설치된 센서퓨전 시스템을 도시한 것으로서, 이동물체(mobile body, 200)에 두 개의 콤파스(210, 220)과 자이로(230), 두 개의 휠 인코더(odometer, 240, 250)를 구비하고 있다.

사용된 센서는 각각 다음과 같은 특성을 가진다. 자이로(230)는 바이어스 드리프트 에러(Bias drift error)와 각도 값을 계산하기 위해 필요한 적분과정에서 장기적으로는 적분에러(integration error)가 발생하나, 단기적으로는 비교적 정확한 각도 값을 검출할 수 있다. 반면에 콤파스(210, 220)는 장기적으로는 절대 방위각을 줄 수 있으므로 정확한 정보를 제공하나, 단기적으로는 생활환경에서 존재하는 자기장 교란(magnetic disturbance)으로 인한 에러를 발생시킬 수 있다. 한편 이동물체의 구동부(로봇의 경우에는 바퀴)에 들어가는 휠 인코더(240, 250)는 장기적으로는 미끄러짐(slippage)에 의한 에러와 휠(wheel)크기 및 정렬(alignment)여부 등의 동역학적인(kinematic) 조건에 의한 에러가 누적되어 역시 제한되지 않는 에러(unbounded error) 특성을 보여주고, 또한 단기적으로도 범프(bump) 충돌이나 공중이동(kidnapping) 같은 돌발적인 상황에 대처할 수 없는 한계를 가지고 있다. 그러나 대부분의 시간 동안 비교적 정확한 정보를 알려주게 된다.

상기 콤파스(210, 220)로부터는 자기장의 방향과 자기장의 크기 데이터를 얻을 수 있다. 상기 자이로(230)는 각속도를 검출할 수 있으며, 휠 인코더(240, 250)로부터 휠의 회전속도를 검출하여 오도미터를 이용한 이동물체의 각속도를 구할 수 있다.

본 발명은 콤파스, 자이로, 오도미터 등의 절대센서와 상대센서를 조합하여 자기장, 장애물, 미끄러운 바닥 등 각종 불확실한 환경에 효과적으로 대처하면서 물체의 Yaw Angle 을 획득하여 위치 및 자세를 최적으로 추정하기 위해 규칙 기반 칼만 필터(rule-based indirect Kalman filter)를 이용한다.

도 3은 본 발명에 의한 이동물체의 진행방향 추정 시스템에 대한 일실시예를 블록도로 도시한 것으로서, 자기장산출부(300), 자기장비교부(310), 지구자기영역 판단부(320) 및 진행방향추정부(330)를 포함하여 이루어진다. 상기 이동물체는 자이로, 오도미터 및 콤파스가 설치되어 있다.

이동물체는 콤파스, 자이로 및 오도미터로부터 센서데이터를 얻는다. 그 중에서 콤파스 정보로부터 자기장을 구한다. 자기장산출부(300)는 상기 콤파스가 설치된 이동물체의 자기장의 크기를 계산한다. 자기장비교부(310)는 상기 자기장의 크기와 지구자기장의 크기의 차를 구하여 소정의 값()과 비교한다. 지구자기영역 판단부(320)는 상기 비교 결과에 따라 이동물체가 존재하는 지점이 지구자기가 작용하는 영역에 속했는지 여부를 판단한다. 진행방향추정부(330)는 상기 판단 결과에 따라 콤파스의 방위각을 이동물체의 방향추정에 이용할지를 결정하여 이동물체의 방향을 추정한다. 두 개의 콤파스로부터 얻은 자기장과 지구자기장의 차를 상기 자기장비교부(310)에서 비교한 결과 어느 하나라도 상기 소정의 값보다 작은 경우에는 콤파스데이터로부터 얻은 방위각을 이동물체의 방향추정에 이용하고, 그렇지 않은 경우에는 자이로와 오도미터의 각속도를 이용하여 이동물체의 방향추정에 이용한다. 이하 이를 보다 상세히 설명한다.

도 4는 본 발명에 의한 이동물체의 진행방향 추정방법에 대한 일실시예를 흐름도로 도시한 것으로서, 이를 바탕으로 본 발명을 구체적으로 설명하기로 한다.

콤파스, 자이로, 오도미터의 센서 데이터를 읽는다.(400단계) 먼저 상기 센서 데이터 중 콤파스 데이터로부터 자기장을 구한다. 도 5는 콤파스 정보로부터 자기장을 구하는 것을 나타내고 있다. 콤파스를 통해 알 수 있는 x축 성분의 자기장(Hx)과 y축 성분의 자기장(Hy)으로부터 자기장 크기(|H|)를 구할 수 있다. 이를 위해 를 수학식 1과 같이 정의하기로 한다. 여기서 i 는 콤파스의 식별기호(ID)이고, H_E 는 지구자기장을 나타낸다.

그리고 수학식 2와 같이 정의하기로 한다.

만일 이면,

여기서 는 임계값(threshold value)을 나타내고, S_H는 지구자기가 작용하는 영역으로서, 성공영역(sucess region)을 나타낸다.

수학식 1과 수학식 2와 같이 정의한 후, 도 6에 나타낸 바와 같이 콤파스1과 콤파스2 데이터를 읽어 H₁및 H₂를 각각 구하여(600단계), 상기 H₁과 H₂가 S_H 에 속하는지를 체크한다.(410단계)

상기 이동물체의 첫 번째 콤파스 자기장(H₁) 크기 및 두 번째 콤파스 자기장(H₂) 크기를 각각 상기 지구자기장(H_E) 크기와 비교하여 그 차가 둘 다 소정의 한계 값 보다 작은 경우(610단계)에는, 도 7에 도시된 바와 같이 가중분할방식 (Weighted Partition Method)을 사용하여 콤파스의 최종 방위각을 계산한다.(420단계) 상기 첫번째 콤파스의 방위각()과 두번째 콤파스의 방위각()의 차를 구한다.(700단계) 상기 방위각의 차가 소정의 값()보다 작은 경우에는 지구자기장과 콤파스들의 자기장의 차에 따라 가중치를 달리하여 이동물체의 콤파스 방위각을 구한다. 즉 와 의 곱을 구해 그 값이 음수이면(710단계) 수학식 3에 의해 구해지는 값을 콤파스의 최종 방위각()으로 한다.(720단계)

만일 상기 610단계에서 구한 값이 양수이면(730단계), 수학식 4에 의해 구해지는 값을 콤파스의 최종 방위각()으로 한다.(740단계)

만일 상기 610단계에서 와 의 곱을 구해 그 값이 0 이면, 수학식 5에 의해 구해지는 값을 콤파스의 최종방위각()으로 한다.(750단계)

한편, 상기 600단계에서 상기 첫번째 콤파스의 방위각()과 두번째 콤파스의 방위각( )의 차가 소정의 값()보다 작지 않은 경우에는 수학식 6을 만족하는지를 체크한다.(760단계)

상기 수학식 6을 만족하면, 콤파스1의 방위각( )을 콤파스의 최종 방위각 ()으로 한다.(770단계) 만일 수학식 6을 만족하지 않고 수학식 7을 만족하면(780단계), 콤파스2의 방위각()를 콤파스의 최종 방위각()으로 한다.(790단계)

만일 760단계 및 780단계에서 등식이 성립하면, 750단계에서와 같이 수학식 5에 의해 구해지는 값을 콤파스의 최종 방위각()으로 한다.

상기 수학식 3 내지 7에서, , 은 샘플링 주기 동안 콤파스 1 및 콤파스 2의 방위각 변화량이며, 는 자이로의 각속도이며, 는 샘플링 시간을 의미한다.

한편 도 4의 450단계를 보다 상세히 설명하면 도 6과 도 8과 같다. 도 6에서 상기 이동물체의 첫 번째 콤파스 자기장(H₁) 크기 및 두 번째 콤파스 자기장(H₂) 크기를 각각 상기 지구자기장(H_E) 크기와 비교하여 첫 번째 콤파스 자기장(H₁) 크기는 소정의 한계값 보다 작지만, 두 번째 콤파스 자기장(H₂) 크기는 소정의 한계 값보다 작지 않은 경우에는(620단계), 도 8에 도시된 바와 같이 콤파스1의 방위각( )을 콤파스의 최종 방위각( )으로 한다.(800단계)

그리고 도 6에서 상기 이동물체의 첫 번째 콤파스 자기장(H₁) 크기 및 두 번째 콤파스 자기장(H₂) 크기를 각각 상기 지구자기장(H_E) 크기와 비교하여 두 번째 콤파스 자기장(H₁) 크기는 소정의 한계값 보다 작지만, 첫 번째 콤파스 자기장(H₂) 크기는 소정의 한계값보다 작지 않은 경우에는(630단계), 콤파스2의 방위각()을 콤파스의 최종 방위각()으로 한다.(810단계)

그리고 상술한 방법에 의해 구해진 콤파스의 최종 방위각()은 에러상태 피드백 칼만필터를 통해(430단계), 최적의 추정된 진행방향을 구하는데 이용된다.(440단계)

한편 도 6에서 상기 이동물체의 첫 번째 콤파스 자기장(H₁) 크기 및 두 번째 콤파스 자기장(H₂) 크기를 각각 상기 지구자기장(H_E) 크기와 비교하여 둘다 소정의 한계값 보다 작지않은 경우에는 콤파스 데이터를 사용하지 않고 자이로와 오도미터의 각속도의 차를 체크한다.(460단계, 820단계)

한편 도 4의 460단계 내지 495단계를 보다 상세히 설명하면 도 9와 같다. 도 9는 자이로와 오도미터의 각속도를 체크하여 이동물체의 진행방향을 추정하는 과정을 흐름도로 도시한 것이다. 먼저 수학식 8과 같이 오도미터로부터 각속도( )를 계산한다.(900단계)

여기서 V_r,V_l 은 휠(wheel)의 속도이고 l 은 트래드 길이(Tread length) 즉 휠 간의 거리를 나타낸다.

자이로 각속도()와 오도미터의 각속도()의 차가 소정의 값() 이내인지 체크하여(910단계), 소정의 값 이내이면 오도미터 데이터를 가지고 에러상태 피드백 칼만필터를 구동한다.(920단계) 그리고 나서 최적의 진행방향을 추정한다.(930단계)

그리고 상기 자이로 각속도와 오도미터의 각속도를 차가 소정의 값 이내인지 체크하여(910단계), 소정의 값 보다 작지 아니하면 칼만필터를 사용하지 아니하고 자이로의 각 속도를 적분하여(940단계), 부최적의(suboptimal) 진행방향을 계산한다.(950단계) 그리고 실패인덱스(Failure index)의 값을 증가시키고(960단계), 상기 인덱스가 소정의 임계값을 초과하면(970단계), 경고를 하고 시스템의 작동을 중단시킨다.(980단계) 그리고 상기 실패인덱스가 소정의 경계값보다 작으면 콤파스, 자이로 및 오도미터의 센서 데이터를 읽는다.

한편, 오도미터와 이동물체의 일예에 해당하는 로봇의 모델링을 다음과 같이 할 수 있다.

도 10은 로봇의 위치를 2차원 좌표로 도시한 것으로서, Ts를 샘플링 시간이라 할 때, 상기 로봇을 모델링하면 수학식 9와 같다.

그리고 오도미터를 모델링하면, 휠의 지름을 D_l,r, 휠의 1회전당의 펄스수인 인코더 resolution을 C_e, 기어 Ratio를 n, 샘플링 간격내의 펄스 수를 N_l,r라 할 때, 수학식 10과 같다.

그리고 오도미터의 각속도는 수학식 11과 같다.

다음으로 자이로를 모델링하면 다음과 같다. 먼저 자이로의 실제 모델(Gyro Actual Model)은, 이동물체의 진행방향(heading direction)을 , 자이로 드리프트 율을 b, 자이로 출력을 , 분산 를 갖는 가우시안 백색 잡음을 , 분산 을 갖는 가우시안 백색잡음을 n_b 라 할 때, 수학식 12과 같이 나타낼 수 있다.

그리고 자이로의 추정 모델 즉 적분기의 출력은, 진행방향을 , 추정된 자이로 드리프트를 (상수라 가정)라 할 때, 수학식 13과 같이 표현할 수 있다.

다음으로 에러상태를 모델링하면 다음과 같다. 먼저 에러 상태를 수학식 14와 같이 정의한다.

외부 센서측정 모델은, 측정된 진행방향을 , 분산 을 갖는 가우시안 백색 측정 잡음을 라 할 때, 수학식 15와 같이 나타낼 수 있다. 여기서 외부센서측정입력은 상기 서술한 방법에 따라 콤파스의 최종방위각, 오도미터 각속도 중에서 선택한다.

그리고 에러상태 모델은 수학식 13에서 수학식 12를 빼면 수학식 16과 같이 된다.

도 11은 에러상태 측정 모델을 도시한 것이다.

이상의 모델을 이산(discrete) 모델로 표현하면 다음과 같다. 먼저, 이산 에러상태를 기술한다. 이산 에러상태는 수학식 17과 같이 정의할 수 있다.

상기 수학식 12의 이산 형태는 수학식 18과 같이 표현할 수 있다.

상기 수학식 13의 이산 형태는 수학식 19로 표현할 수 있다.

상기 수학식 18 및 수학식 19로부터 수학식 20을 이끌어 낼 수 있다.

수학식 15의 이산 형태는 수학식 21과 같이 표현할 수 있다.

이산 에러상태 칼만필터를 다음과 같이 상세히 기술한다. 초기조건은 수학식 22 및 수학식 23과 와 같이 나타낼 수 있다. 수학식 22는 근원적으로 평균값이 0 인 가우시안 랜덤변수(RV) 이며, 수학식 23은 초기값 , b의 분산을 나타낸 것이다.

가우시안 잡음의 이산형태는 수학식 24와 같이 표현할 수 있다.

시간 갱신에 대한 이산 형태는 수학식 25 및 수학식 26과 같이 나타낼 수 있다.

측정 갱신(measurement update)에 대한 이산형태는 수학식 27, 수학식 28 및 수학식 29와 같이 표현할 수 있다.

수학식 19로부터 수학식 30을 이끌어 낼 수 있고, 수학식 28로부터 수학식 31을 도출할 수 있다.

라고 정의할 때, 수학식 30을 수학식 31에 더하므로써 수학식 32를 얻을 수 있다.

초기조건은 수학식 33과 같이 된다.

수학식 32는 본 발명에서 쓰이는 에러상태 피드백 칼만필터의 대표 수학식을 나타낸다. 서술한 방법에 따라 콤파스의 방위각, 오도미터의 각속도 중에서 측정입력을 선택하여 수학식 32의 에 대입하면 이동방향의 최적추정치 을 구할 수 있게 된다.

본 발명은 컴퓨터로 읽을 수 있는 기록 매체에 컴퓨터(정보 처리 기능을 갖는 장치를 모두 포함한다)가 읽을 수 있는 코드로서 구현하는 것이 가능하다. 컴퓨터가 읽을 수 있는 기록 매체는 컴퓨터 시스템에 의하여 읽혀질 수 있는 데이터가 저장되는 모든 종류의 기록 장치를 포함한다. 컴퓨터가 읽을 수 있는 기록 장치의 예로는 ROM, RAM, CD-ROM, 자기 테이프, 플로피 디스크, 광데이터 저장장치 등이 있다.

본 발명은 도면에 도시된 실시예를 참고로 설명되었으나 이는 예시적인 것에 불과하며, 본 기술 분야의 통상의 지식을 가진 자라면 이로부터 다양한 변형 및 균등한 타 실시예가 가능하다는 점을 이해할 것이다. 따라서, 본 발명의 진정한 기술적 보호 범위는 첨부된 등록청구범위의 기술적 사상에 의해 정해져야 할 것이다.

본 발명에 의한 센서퓨전 방법은 센서의 특성보다는 센서의 퓨전이 중요한 요소로 작용하므로 높은 사양의 센서가 필요가 없어서 저가형 센서로 고성능을 구현하는 것이 가능하다. 자이로, 콤파스, 오도미터 센서 각각의 단점을 커버하고, 장점을 취함으로써 이동물체의 이동시 발생할 수 있는 많은 돌발상황에 대한 대처가 가능하고, 다양한 종류의 에러원(Error Source)를 추정할 수 있게 해준다. 센서의 에러특성에 대한 정확한 정보가 필요없고, 바이어스, 유색 노이즈(colored noise), 비시스템적인 에러(nonsystematic error)등에 대한 대처가 가능하는 등 칼만필터가 가지고 있는 여러 가지 고유의 한계를 극복할 수 있다. 부수적인 효과로서 시스템 폴트(System Fault)의 감지기능이 구현가능해진다.

본 발명은 Yaw Angle을 효과적으로 구할 수 있게 함으로써, 3D 각도 정보를 견실하게 제공해주므로, 절대 위치 및 자세 추정이 필요한 모든 분야에 적용 할 수 있다. 적용가능 분야는 자율주행, 지능형 차량(Intelligent Vehicle), 차량항법(Car Navigation), 의료 로봇(Medical Robot), VR, Entertainment, 무인 비행기, 개인 항법(Personal Navigation) 등이 있다.

도 1은 (x,y) 위치 및 방향 θ를 추정하기 위한 2차원 평면에서 움직이는 이동로봇을 도시한 것이다.

도 2는 본 발명에 의한 센서 퓨전 시스템의 개념도를 도시한 것이다.

도 3은 본 발명에 의한 이동물체의 진행방향 추정 시스템에 대한 일실시예를 블록도로 도시한 것이다.

도 4는 본 발명에 의한 이동물체의 진행방향 추정방법에 대한 일실시예를 흐름도로 도시한 것이다.

도 5는 콤파스 정보로부터 자기장을 구하는 것을 도시한 것이다.

도 6은 콤파스 1과 콤파스 2에 의해 구해진 H₁과 H₂가 S_H 에 속하는지를 흐름도로 도시한 것이다.

도 7은 가중분할방식(Weighted Partition Method)을 사용하여 이동물체의 진행방향 계산을 흐름도로 도시한 것이다.

도 8은 도 4에서 구한 콤파스의 자기장과 지구자기장의 차가 소정의 값보다 작지 않을 경우 이동물체의 진행방향 결정을 흐름도로 도시한 것이다.

도 9은 자이로와 오도미터의 각속도를 체크하여 이동물체의 진행방향을 추정하는 과정을 흐름도로 도시한 것이다.

도 10은 로봇의 위치를 2차원 좌표로 도시한 것이다.

도 11은 에러상태 측정 모델을 도시한 것이다.

Claims

이동물체의 콤파스 정보로부터 자기장 정보를 구하는 제1단계;

상기 이동물체의 자기장을 소정의 값과 비교하여, 그 결과에 따라 이동물체가 존재하는 지점이 특정 영역에 속했는지 여부를 판단하는 제2단계; 및

상기 판단 결과에 따라 콤파스의 방위각을 이동물체의 방향추정에 이용할지의 여부를 결정하여 이동물체의 방향을 추정하는 제3단계를 포함함을 특징으로 하는 이동물체의 진행방향 추정방법.
제1항에 있어서, 상기 소정의 값은 지구자기장의 크기이고,

상기 특정 영역은 지구자기가 작용하는 영역임을 특징으로 하는 이동물체의 진행방향 추정방법.
제2항에 있어서, 상기 제2단계는

상기 이동물체에 두 개의 콤파스가 설치되어 있을 경우, 이동물체의 첫번째 콤파스의 자기장 크기 및 두번째 콤파스 자기장 크기를 각각 상기 지구자기장 크기와 비교하여 그 차가 둘 다 소정의 한계 값 보다 작은 경우, 하나만 소정의 한계 값보다 작은 경우, 둘 다 소정의 한계 값보다 작지 않은 경우로 나누어서 그 결과에 따라 이동물체가 존재하는 지점이 특정 영역에 속했는지 여부를 판단함을 특징으로 하는 이동물체의 진행방향 추정방법.
제3항에 있어서, 상기 제3단계는

이동물체가 특정영역에 속하여 콤파스의 정보가 유효하다고 판단할 경우 콤파스의 최종방위각을 구하고 상기 방위각을 측정입력으로 채택한 칼만필터를 이용하여 최적의 진행방향각도를 추정하고,

이동물체가 특정영역에 속하지 아니하여 콤파스의 정보가 유효하지 않다고 판단할 경우 자이로와 오도미터의 각속도를 비교하여 그 차이가 소정의 값보다 작을 경우 오도미터로부터 구한 방향을 측정입력으로 채택한 칼만필터를 이용하여 최적의 진행방향을 추정하거나, 상기 차이가 소정의 값보다 작지 않을 경우 자이로의 각속도를 적분하여 진행방향을 추정하는 단계임을 특징으로 하는 이동물체의 진행방향 추정방법.
제3항에 있어서, 상기 이동물체의 첫번째 콤파스 자기장 크기 및 두번째 콤파스 자기장 크기를 각각 상기 지구자기장 크기와 비교하여 그 차가 둘 다 소정의 한계 값 보다 작은 경우의 이동물체의 방향추정은

상기 첫 번째 콤파스의 방위각과 두 번째 콤파스의 방위각의 차를 구하는 단계;

상기 방위각의 차가 소정의 값보다 작은 경우에는, 지구자기장과 콤파스들의 자기장의 차에 따라 가중치를 달리하여 이동물체의 콤파스 방위각을 구하는 가중치단계; 및

상기 방위각의 차가 소정의 값보다 작지 않은 경우에는, 이동물체에 대한 자이로의 각속도와 콤파스의 방위각에 대한 각속도의 차를 각각 구해 그 크기에 따라 이동물체의 콤파스 방위각을 구하는 각속도단계를 포함함을 특징으로 하는 이동물체의 진행방향 추정방법.
제5항에 있어서, 상기 가중치 단계는

, 을 샘플링 주기 동안 콤파스 1 및 콤파스 2의 방위각 변화량, 는 자이로의 각속도, 을 샘플링 시간이라 하고, 상기 첫 번째 콤파스의 자기장크기와 지구자기장 크기의 차를 , 상기 두 번째 콤파스의 자기장크기와 지구자기장 크기의 차를 라 할 때,

상기 와 의 곱을 구해 그 값이 음수이면, 수학식 3에 의해 구해지는 값을 콤파스의 최종방위각( )으로 하고,

[수학식 3]

만일 상기 와 상기 의 곱이 양수이면, 수학식 4에 의해 구해지는 값을 콤파스의 최종방위각( )로 하고,

[수학식 4]

[수학식 5]

만일 상기 곱의 값이 0 이면, 수학식 5에 의해 구해지는 값을 콤파스의 최종방위각()으로 함을 특징으로 하는 이동물체의 진행방향 추정방법.
제5항에 있어서, 상기 각속도 단계는

, 을 샘플링 주기 동안 콤파스 1 및 콤파스 2의 방위각 변화량, 는 자이로의 각속도, 을 샘플링 시간이라 할 때,

상기 첫번째 콤파스의 방위각()과 두번째 콤파스의 방위각()의 차가 소정의 값보다 작지 않은 경우에는 수학식 6을 만족하는지를 체크하는 단계;

[수학식 6]

상기 수학식 6을 만족하면, 콤파스1의 방위각()을 콤파스의 최종방위각()로 하는 단계;

만일 수학식 6을 만족하지 않고 수학식 7을 만족하면, 콤파스2의 방위각()를 콤파스의 최종 방위각()으로 하는 단계;

[수학식 7]

만일 상기 수학식 6과 수학식 7에서 양변이 서로 같으면

[수학식 5]

상기 수학식 5에 의해 구해지는 값을 콤파스의 최종 방위각()로 함을 특징으로 하는 이동물체의 진행방향 추정방법.
제3항에 있어서, 상기 이동물체의 첫번째 콤파스 자기장 크기 및 두번째 콤파스 자기장 크기를 각각 상기 지구자기장 크기와 비교하여 그 차 중 하나만 소정의 한계값 보다 작은 경우의 이동물체의 방향추정은

상기 한계값보다 작은 콤파스에 대한 방위각을 콤파스의 최종 방위각으로 함을 특징으로 하는 이동물체의 진행방향 추정방법.
제3항에 있어서, 상기 이동물체의 첫번째 콤파스 자기장 크기 및 두번째 콤파스 자기장 크기를 각각 상기 지구자기장 크기와 비교하여 그 차가 둘 다 소정의 한계값 보다 작지 않은 경우의 이동물체의 방향추정은

콤파스 데이터를 사용하지 않고 이동물체의 휠속도에 대한 각속도(오도미터에 의해 구해진 각속도)를 계산하는 단계;

상기 이동물체의 자이로 각속도와 상기 휠의 각속도의 차를 구하는 단계;

상기 자이로 각속도와 휠 각속도의 차가 소정의 값보다 작으면, 오도미터로부터 구한 방향을 측정입력으로 채택한 칼만필터를 이용하여 최적의 진행방향각도를 추정하는 단계; 및

상기 자이로 각속도와 휠 각속도의 차가 소정의 값보다 작지 않으면, 칼만필터를 사용하지 않고 자이로의 각속도를 적분하여 진행방향각도를 추정하는 적분추정단계를 포함함을 특징으로 하는 이동물체의 진행방향 추정방법.
제9항에 있어서, 상기 적분추정단계는

소정 시간 내에 상기 적분추정단계가 소정 횟수 이상 수행될 경우에는 시스템을 종료하는 단계를 더 구비함을 특징으로 하는 이동물체의 진행방향 추정방법.
제5항 또는 제8항에 있어서,

에러 상태 피드백 칼만 필터를 통해 이동물체의 진행방향 각도를 필터링하여 이동물체에 대한 최적의 진행방향 추정치를 계산하는 단계를 더 구비함을 특징으로 하는 이동물체의 진행방향 추정방법.
제1항 내지 제11항 어느 한 항에 기재된 발명을 컴퓨터에서 실행시키기 위한 프로그램을 기록한 컴퓨터로 읽을 수 있는 기록매체.
자이로, 오도미터 및 콤파스가 설치된 이동물체에 있어서,

상기 콤파스가 설치된 이동물체의 자기장의 크기를 계산하는 자기장산출부;

상기 자기장의 크기와 지구자기장의 크기의 차를 구하여 소정의 값과 비교하는 자기장비교부;

상기 비교 결과에 따라 이동물체가 존재하는 지점이 지구자기가 작용하는 영역에 속했는지 여부를 판단하는 지구자기영역 판단부; 및

상기 판단 결과에 따라 콤파스의 방위각을 이동물체의 방향추정에 이용할지를 결정하여 이동물체의 방향을 추정하는 진행방향추정부를 포함함을 특징으로 하는 이동물체의 진행방향 추정시스템.