KR20130143576A

KR20130143576A - 중력 기준계에서의 장치에 부착된 움직임 센서 및 자력계의 측정치를 사용하여 장치의 요각을 추정하는 장치 및 방법

Info

Publication number: KR20130143576A
Application number: KR1020137011278A
Authority: KR
Inventors: 후아 셍
Original assignee: 힐크레스트 래보래토리스, 인크.
Priority date: 2010-10-01
Filing date: 2011-09-30
Publication date: 2013-12-31
Also published as: CN103153790B; WO2012044964A2; WO2012044964A3; CN103153790A; US20130185018A1; EP2621809A4; EP2621809A2

Abstract

장치에 부착된 움직임 센서 및 자력계를 사용하여 중력 기준계에 대한 장치의 몸체 기준계의 요각을 추정하는 방법이 제공된다.　 방법은 (A) 움직임 센서 및 자력계로부터 측정치를 수신하는 단계, (B) 수신된 측정치에 기초하여, 측정된 3-D 자계, 롤, 피치, 및 몸체 기준계에서의 요의 원시 추정치를 구하는 단계, (C) 측정된 3-D 자계로부터 국소 3-D 자계를 추출하는 단계, 및 (D) 적어도 2개의 상이한 방법을 사용하여, 추출된 국소 3-D 자계, 롤, 피치 및 요의 원시 추정치에 기초하여 중력 기준계에서의 몸체 기준계의 요각을 계산하는 단계를 포함하고, 롤, 피치 및 추출된 국소 3-D 자계의 추정된 오차가 상이한 방법에 대해 상이하게 요의 오차에 영향을 미친다.

Description

중력 기준계에서의 장치에 부착된 움직임 센서 및 자력계의 측정치를 사용하여 장치의 요각을 추정하는 장치 및 방법{APPARATUSES AND METHODS FOR ESTIMATING THE YAW ANGLE OF A DEVICE IN A GRAVITATIONAL REFERENCE SYSTEM USING MEASUREMENTS OF MOTION SENSORS AND A MAGNETOMETER ATTACHED TO THE DEVICE}

관련 출원

이 출원은 2011년 10월 1일자로 출원된, 발명의 명칭이 “자력계-기반 감지(Magnetometer-Based Sensing)”인 미국 가특허 출원 제61/388,865호; 2011년 11월 17일자로 출원된, 발명의 명칭이 “경사각 및 초기 요각에 대한 사전 지식 없이 자력계 정렬 보정(Magnetometer Alignment Calibration Without Prior Knowledge of Inclination Angle and Initial Yaw Angle)”인 미국 가특허 출원 제61/414,560호; 2011년 11월 17일자로 출원된, 발명의 명칭이 “닫힌 형태의 자력계 자세 독립 파라미터 교정(Magnetometer Attitude Independent Parameter Calibration In Closed Form)”인 미국 가특허 출원 제61/414,570호; 및 2011년 11월 17일자로 출원된, 발명의 명칭이 “동적 근방 자계 추적 및 보상(Dynamic Magnetic Near Field Tracking and Compensation)”인 미국 가특허 출원 제61/414,582호(이들의 개시 내용은 참조 문헌으로서 본 명세서에 포함됨)에 관한 것으로서, 이들에 기초하여 우선권을 주장한다.

기술분야

본 발명은 일반적으로 자력계 및 다른 움직임 센서의 측정치를 사용하여, 중력 기준계(gravitational reference system)에서의 장치의 요각(yaw angle)을 추정하고 및/또는 동적 근방계(dynamic near field)에 대해 보정된 정적 자계를 추출하는 데 사용되는 파라미터를 결정하는 장치 및 방법에 관한 것이다. 보다 구체적으로는, 자력계에 의해 획득된 신호를 자력계의 오프셋, 스케일 및 교차 결합/스큐, 강철 및 연철 효과 그리고 정렬 편차를 보정하는 국소 자계로 변환하는 데 사용되는 파라미터가 동시적인 측정치를 사용하여 적어도 부분적으로 분석적으로 추출된다. 중력 기준계에서의 장치의 요각이 국소 정적 자계(즉, 추적된 근방계가 제거된 국소 자계)를 사용하여 실시간으로 추정되고, 현재의 롤 및 피치가 동시적인 측정치에 기초하여 추출된다.

점점 더 보편화되고 확산된 모바일 장치가 종종 소위 9축 센서 - 이 이름은 3축 자이로스코프, 3-D 가속도계, 및 3-D 자력계로부터 얻어짐 - 를 포함한다. 3-D 자이로스코프는 각속도를 측정한다. 3-D 가속도계는 선가속도를 측정한다. 자력계는 국소 자계 벡터(또는 그의 편차)를 측정한다. 그의 보편화에도 불구하고, 이들 9축 센서의 예측가능한 기능은 한쪽 손에 있는 자력계 측정치로부터의 바람직하지 않은 효과를 교정하고 제거하는 것의 어려움 그리고 자이로스코프 및 가속도계만을 사용하여 요각을 신뢰성있게 추정하는 것이 실제로 불가능한 것으로 인해 완전히 이용되지 않는다.

지구-고정 중력 직교 기준계에 대한 강체의(즉, 자력계 및 움직임 센서가 부착되어 있는 임의의 장치를 지정하는 강체에 의한) 3-D 각위치가 일의적으로 정의된다. 자력계 및 가속도계가 사용될 때, 중력 기준계을 중력을 따라 있는 플러스 Z-축, 자북을 가리키는 플러스 X-축 및 동쪽을 가리키는 플러스 Y-축을 갖는 것으로 정의하는 것이 편리하다. 가속도계가 중력을 감지하지만, 자력계의 측정치로부터, 중력이 북쪽을 가리키는 지구의 자계로부터 추론될 수 있다(그렇지만, 지구의 자계와 중력 사이의 각도가 90º와 다를 수 있는 것으로 알려져 있음). 중력 기준계의 축을 정의하는 이러한 방식은 제한하기 위한 것이 아니다. 직교 오른손 기준계(orthogonal right-hand reference system)의 다른 정의가 2개의 공지된 방향인 중력과 자북에 기초하여 도출될 수 있다.

3-D 몸체에 부착된 움직임 센서는 3-D 몸체에 대해 정의된 몸체 직교 기준계(body orthogonal reference system)에서의 그의 위치(또는 그의 변화)를 측정한다. 예를 들어, 항공기에 대해 도 1에 예시된 바와 같이, 일반성의 상실 없이, 몸체 기준계(body reference system)는 항공기의 세로축을 따라 전방으로 향해 있는 X-축을 가지며, 플러스 Y-축은 우측 날개를 따라 향해 있고, 플러스 Z-축은 오른손 직교 기준계(오른손 법칙)를 고려하여 결정된다. 항공기가 수평으로 나는 경우, 플러스 Z-축은 중력을 따라 중력 시스템의 Z-축에 맞춰 정렬된다. 중력 기준계에서의 롤 및 피치가 몸체에 부착되어 있는 3-D 가속도계 및 2 또는 3-D 회전 센서를 사용하여 그리고 중력의 공지된 방향에 기초하여 결정될 수 있지만(예컨대, Liberty 특허 - 미국 특허 제7,158,118호, 제7,262,760호 및 제7,414,611호 참조), 중력 기준계에서의 요각은 정확하게 추정하기가 더 어렵고, 자력계 측정치로부터 지구의 자계(또는 보다 정확하게는 그의 배향)를 사용하여 그 판독치를 보강하는 것이 바람직하다.

오일러(Euler)의 정리에 기초하여, 몸체 기준계 및 중력 기준계(2개의 직교 오른손 좌표계임)는 좌표축을 중심으로 한 (3번 이하의) 회전 시퀀스에 의해 관련될 수 있으며, 여기서 연속적인 회전은 상이한 축을 중심으로 한 것이다. 이러한 회전 시퀀스를 오일러각-축 시퀀스(Euler angle-axis sequence)라고 한다. 이러한 기준 회전 시퀀스가 도 2에 예시되어 있다. 이들 회전의 각도는 중력 기준계에서의 장치의 각위치이다.

3-D 자력계는 3-D 정적 자계(예컨대, 지구의 자계)의 중복을 나타내는 3-D 자계, 강철 및 연철 효과, 그리고 외부의 시간 의존적 전자계로 인한 3-D 동적 근방계를 측정한다. 측정된 자계는 자력계의 실제 배향에 의존한다. 강철 효과, 연철 효과 및 동적 근방계가 0인 경우, (자력계가 상이한 방향으로 배향되어 있을 때) 측정된 자계의 궤적은 지구의 자계의 크기와 동일한 반경의 구이다. 영이 아닌 강철 및 연철 효과는 측정된 자계의 궤적을 원점으로부터 오프셋된 타원체로 만든다.

강철 효과는 지구의 자계와 중복하는 일정한 자계를 나타내고 그로써 측정된 자계의 성분의 일정한 오프셋을 발생하는 물질에 의해 생성된다. 자력계에 대한 강철 효과로 인한 자계의 소스의 배향 및 위치가 일정한 한, 대응하는 오프셋도 역시 일정하다.

지구의 자계와 중복하는 자계를 산출하는 강철 효과와 달리, 연철 효과는 자계에 영향을 미치거나 자계를 왜곡시키지만 자계 자체를 꼭 발생할 필요는 없는 물질(철 및 니켈 등)의 결과이다. 따라서, 연철 효과는 자력계 및 지구의 자계에 대한 효과를 야기하는 물질의 위치 및 특성에 따른 측정된 자계의 왜곡이다. 이와 같이, 간단한 오프셋으로는 연철 효과가 보상될 수 없고, 보다 복잡한 절차를 필요로 한다.

근방 자계는 시간 의존적 자계로 인한 측정된 자계의 동적 왜곡이다. 3축 가속도계 및 3축 회전 센서로부터 요에 대한 신뢰성있는 추정이 없는 경우(예컨대, 절대 요각 측정치에 대한 어떤 관측도 없음으로 인한 요각 드리프트 문제), 근방 자계 보상된 자력계의 측정치는 요각 드리프트를 보정하는 것을 가능하게 해주는 중요한 기준을 제공할 수 있다.

종래에는, 강철 효과 및 연철 효과가 복수의 자계 측정치를 사용하여 보정된다. 이 방식은 시간 및 메모리를 소비한다. 그에 부가하여, 강철 효과 및 연철 효과에 의해 야기된 왜곡의 동적 특성이 주어지면, 복수의 자기 측정치의 차이가 또한 현재의 측정치를 과잉 보정하거나 과소 보정하는 것을 야기하는 시간에 따른 국소 자계의 변화를 반영할 수 있다.

따라서, 상기한 문제 및 단점을 피하면서 장치의 배향(즉, 요각을 포함하는 각위치)을 결정하기 위해 장치에 부착된 다른 움직임 센서와 함께 자력계의 신뢰성있는 실시간 사용을 가능하게 해주는 장치, 시스템 및 방법을 제공하는 것이 바람직하다.

자력계를 포함하는 센서들의 조합으로부터의 동시적인 측정치를 사용하는 장치, 시스템 및 방법은 국소 3-D 정적 자계값, 그리고 이어서 3-D 몸체의 향상된 요각 값을 산출한다.

한 예시적인 실시예에 따르면, 장치에 부착된 움직임 센서 및 자력계를 사용하여 중력 기준계에 대한 장치의 몸체 기준계의 요각을 추정하는 방법이 제공된다. 이 방법은 (A) 움직임 센서로부터 그리고 자력계로부터 측정치를 수신하는 단계, (B) 수신된 측정치에 기초하여, 측정된 3-D 자계, 롤(roll), 피치(pitch), 및 몸체 기준계에서의 장치의 요(yaw)의 원시 추정치를 구하는 단계, (C) 측정된 3-D 자계로부터 정적 국소 3-D 자계를 추출하는 단계, 및 (D) 적어도 2개의 상이한 방법을 사용하여, 추출된 국소 3-D 자계, 롤각(roll angle), 피치각(pitch angle) 및 요각의 원시 추정치에 기초하여 중력 기준계에서의 장치의 몸체 기준계의 틸트-보상된 요각을 계산하는 단계를 포함하고, 롤각 추정치의 오차, 피치각 추정치의 오차, 및 추출된 국소 3-D 자계의 오차는 적어도 2개의 상이한 방법에 대해 상이하게 틸트-보상된 요각의 오차에 영향을 미친다.

다른 예시적인 실시예에 따르면, (A) 강체를 가지는 장치, (B) 장치 상에 탑재되어 있고 국소 자계에 대응하는 측정치를 발생하도록 구성되어 있는 3-D 자력계, (C) 장치 상에 탑재되어 있고 강체의 배향에 대응하는 측정치를 발생하도록 구성되어 있는 움직임 센서, 및 (D) 적어도 하나의 처리 유닛을 포함하는 장치가 제공된다. 적어도 하나의 처리 유닛은 (1) 움직임 센서로부터 그리고 자력계로부터 측정치를 수신하고, (2) 수신된 측정치에 기초하여, 측정된 3-D 자계, 롤각, 피치각, 및 몸체 기준계에서의 장치의 요각의 원시 추정치를 구하며, (3) 측정된 3-D 자계로부터 국소 3-D 자계를 추출하고, (4) 적어도 2개의 상이한 방법을 사용하여, 추출된 국소 3-D 자계, 롤각, 피치각 및 요각의 원시 추정치에 기초하여 중력 기준계에서의 장치의 몸체 기준계의 틸트-보상된 요각을 계산하도록 구성되어 있고, 롤각 추정치의 오차, 피치각 추정치의 오차, 및 추출된 국소 3-D 자계의 오차는 적어도 2개의 상이한 방법에 대해 상이하게 틸트-보상된 요각의 오차에 영향을 미친다.

다른 예시적인 실시예에 따르면, 컴퓨터 상에서 실행될 때 컴퓨터로 하여금 장치에 부착된 움직임 센서 및 자력계를 사용하여 중력 기준계에 대한 장치의 몸체 기준계의 요각을 추정하는 방법을 수행하게 하는 실행가능 코드를 비일시적으로 저장하도록 구성되어 있는 컴퓨터 판독가능 저장 매체가 제공된다. 이 방법은 (A) 움직임 센서로부터 그리고 자력계로부터 측정치를 수신하는 단계, (B) 수신된 측정치에 기초하여, 측정된 3-D 자계, 롤(roll), 피치(pitch), 및 몸체 기준계에서의 장치의 요(yaw)의 원시 추정치를 구하는 단계, (C) 측정된 3-D 자계로부터 정적 국소 3-D 자계를 추출하는 단계, 및 (D) 적어도 2개의 상이한 방법을 사용하여, 추출된 국소 3-D 자계, 롤각(roll angle), 피치각(pitch angle) 및 요각의 원시 추정치에 기초하여 중력 기준계에서의 장치의 몸체 기준계의 틸트-보상된 요각을 계산하는 단계를 포함하고, 롤각 추정치의 오차, 피치각 추정치의 오차, 및 추출된 국소 3-D 자계의 오차는 적어도 2개의 상이한 방법에 대해 상이하게 틸트-보상된 요각의 오차에 영향을 미친다.

본 명세서에 포함되어 그의 일부를 구성하는 첨부 도면은 하나 이상의 실시예를 나타내고, 이 설명과 함께, 이들 실시예를 설명한다.
도 1은 3-D 몸체 기준계를 나타낸 도면.
도 2는 중력 기준계로부터 몸체 기준계로의 전환을 나타낸 도면.
도 3은 예시적인 실시예에 따른, 감지 유닛의 블록도.
도 4는 예시적인 실시예에 따른, 틸트-보상된 롤각 및 피치각을 사용하여 요각을 계산하는 방법(300)의 블록도.
도 5는 중력에 대한 지구의 자계의 배향을 나타낸 도면.
도 6은 예시적인 실시예에 따른, 자세-독립적 파라미터를 교정하는 방법의 블록도.
도 7은 예시적인 실시예에 따른, 자세-독립적 파라미터를 교정하는 데 사용될 데이터를 수집하는 데 사용되는 시스템의 블록도.
도 8은 예시적인 실시예에 따른, 지구-고정 중력 기준에 맞춰 3-D 자력계를 정렬하는 방법의 블록도.
도 9는 예시적인 실시예에 따른, 9축 시스템에서 3-D 자력계를 정렬하는 방법의 블록도.
도 10은 예시적인 실시예에 따른, 근방 자계를 추적하고 보상하는 방법의 블록도.
도 11은 예시적인 실시예에 따른, 근방 자계를 추적하고 보상하는 방법의 블록도.
도 12는 예시적인 실시예에 따른, 최상의 요각 추정치를 얻기 위해 요각 추정치들을 융합시키는 방법의 블록도.
도 13은 예시적인 실시예에 따른, 장치에 부착된 움직임 센서 및 자력계를 사용하여 중력 기준계에 대한 장치의 몸체 기준계의 요각을 추정하는 방법의 흐름도.
도 14는 예시적인 실시예에 따른, 장치에 부착된 움직임 센서 및 자력계의 동시적인 측정치를 사용하여 자력계를 교정하는 방법의 흐름도.

예시적인 실시예에 대한 이하의 설명은 첨부 도면을 참조한다. 상이한 도면에서의 동일한 참조 번호는 동일하거나 유사한 구성요소를 나타낸다. 이하의 상세한 설명이 본 발명을 제한하지 않는다. 그 대신에, 본 발명의 범위는 첨부된 특허청구범위에 의해 한정된다. 이하의 실시예는, 간략함을 위해, 3-D 강체("장치")에 부착된 움직임 센서 및 자력계를 포함하는 감지 유닛의 용어 및 구조와 관련하여 논의되고 있다. 그렇지만, 다음에 논의될 실시예가 이들 시스템으로 제한되지 않고 유사한 특성을 갖는 다른 센서 또는 자력계를 포함하는 다른 시스템에서 사용될 수 있다.

본 명세서 전체에 걸쳐 "일 실시예" 또는 "실시예"라는 말은 실시예와 관련하여 기술된 특정의 특징, 구조 또는 특성이 본 발명의 적어도 하나의 실시예에 포함되어 있다는 것을 의미한다. 이와 같이, 본 명세서 전체에 걸쳐 여러 곳에서 나오는 "일 실시예에서" 또는 "실시예에서"라는 표현이 모두 동일한 실시예를 말하는 것은 아니다. 게다가, 하나 이상의 실시예에서 그 특정의 특징, 구조 또는 특성이 임의의 적당한 방식으로 결합될 수 있다.

도 3에 나타낸 예시적인 실시예에 따르면, 장치의 배향을 모니터링하기 위해 장치에 부착되어 있을 수 있는 감지 유닛(100)은 장치의 강체(101)에 부착되어 있는 움직임 센서(110) 및 자력계(120)를 포함한다. 움직임 센서(110) 및 자력계(120)에 의해 수행되는 동시적인 측정은 인터페이스(140)를 통해 데이터 처리 유닛(130)으로 송신되는 신호를 산출한다. 도 3에서, 데이터 처리 유닛(130)은 강체(101) 상에 위치해 있다. 그렇지만, 대안의 실시예에서, 데이터 처리 유닛은 원격지에 있을 수 있고, 자력계 및 움직임 센서로부터의 신호는 장치 상에 위치한 송신기에 의해 데이터 처리 유닛으로 전송된다. 데이터 처리 유닛(130)은 적어도 하나의 프로세서를 포함하고, 수신된 신호를 자계를 포함하는 측정된 양으로 변환하기 위해 교정 파라미터를 사용하여 계산을 수행한다.

장치의 몸체(101)(예컨대, 도 1 참조)에 대해 몸체 좌표계가 정의될 수 있다. 움직임 센서(110) 및 자력계(120)가 강체(101)에 고정되어 부착되어 있는 경우, 이들은 몸체 기준계에서 관측가능한 것(예컨대, 자계, 각속도 또는 선가속도)에 관련된 신호를 발생한다. 그렇지만, 예를 들어, 장치와 독립적인 기준계에서 몸체의 배향을 결정하기 위해, 이들 측정된 양을 관측자 기준계에 관계시킬 수 있어야만 한다. 관측자의 기준계를 관성 기준 프레임으로 그리고 몸체 기준계를 비관성 기준계로 간주할 수 있다. 지구 상에 위치한 관측자의 경우, 중력은 하나의 기준 방향을 제공하고, 자북은 또하나의 기준 방향을 제공한다. 관측자의 기준계가 이들 방향에 대해 정의될 수 있다. 예를 들어, 중력 기준계는 중력을 따라 있는 z-축, 중력과 자북 방향을 포함하는 평면에 있는 y-축, 및 오른손 법칙을 사용하여, 동쪽을 가리키는 x-축을 갖는 것으로 정의될 수 있다. 그렇지만, 이 특정의 정의는 제한하기 위한 것이 아니다. 이하의 설명에서, "중력 기준계"라는 용어는 중력 및 자북을 사용하여 정의된 기준계를 말하는 데 사용된다.

이들 신호는 몸체 기준계에서 측정된 양을 반영한다. 몸체 기준계에서의 이들 측정치는 데이터 처리 유닛(130)에 의해 중력 기준계에 대응하는 양으로 변환되도록 추가로 처리된다. 예를 들어, 회전 센서 및 3-D 가속도계를 사용하여, 중력 직교 기준계에 대한 몸체 기준계의 롤 및 피치가 추론될 수 있다. 중력 직교 기준계에서의 장치의 요각을 정확하게 추정하기 위해, 몸체의 기준계에서 측정된 자계로부터 지구의 자계의 배향을 구하는 것이 필요하다.

몸체 기준계에서 측정된 자계로부터 지구의 자계의 배향을 구하기 위해, 데이터 처리 유닛(130)은 사전 결정된 일련의 동작에서 다양한 파라미터를 사용하여 강철 효과, 연철 효과, 오정렬 및 근방계에 대해 (이상적으로는 교정 파라미터를 사용하여 자력계 신호로부터 계산된) 측정된 3-D 자계를 보정한다. 데이터 처리 유닛(130)이 이들 보정 모두를 완료하면, 얻어진 자계는 타당하게도 지구의 자계에 대응하는 국소 정적 자계인 것으로 가정될 수 있다. 지구의 자계는 자연적으로 "복각(dip angle)"이라고 하는 기지의 각도만큼 중력에 수직인 평면보다 약간 위로 또는 아래로 북쪽을 향하고 있다.

시스템(100)에서 수행될 수 있는 방법들의 툴킷이 이하에 기술되어 있다. 데이터 처리 유닛(130)은, 실행될 때, 시스템(100)으로 하여금 하나 이상의 방법을 수행하게 하는 실행가능 코드를 저장하는 컴퓨터 판독가능 매체(135)에 연결되어 있을 수 있다.

예시적인 실시예에 따르면, 툴킷은 다음과 같은 것들을 포함할 수 있다(이하의 방법 유형들 각각이 본 개시 내용에서 나중에 별도의 섹션에서 기술되어 있다):

(1) 틸트-보상된 요각을 계산하는 방법,

(2) 바이어스(bias), 스케일, 및 스큐 등의 자세-독립적 자력계 파라미터를 결정(교정)하는 방법(교차 결합),

(3) 둘러싸고 있는 연철로부터 얻어지는 등가의 효과를 포함하는 자세-의존적 자력계-정렬 파라미터를 결정(교정)하는 방법,

(4) 동적 근방계를 추적하고 보상하는 방법, 및

(5) 최상의 요각 추정치를 얻기 위해 상이한 요각 추정치들을 융합하는 방법.

이들 방법 중 몇몇은, 자력계 데이터에 부가하여, 중력 기준계에서의 장치의 롤 및 피치각, 그리고 중력 기준계에서의 미지의 초기 오프셋의 영향을 받는 장치의 상대 요각(relative yaw angle)을 사용한다. 중력 기준계에서의 롤각 및 피치각은, 예를 들어, Liberty 특허에 기술된 바와 같이 3-D 가속도계 및 3-D 회전 센서로부터 구해질 수 있다. 그렇지만, 방법 (1) 내지 방법 (5)는 그 방식으로 제한되지 않고, 중력 기준계에서 롤각 및 피치각을 획득하기 위해 특정의 움직임 센서가 사용된다.

방법 (2) 내지 방법 (4)는 자력계에 의해 측정되는 자계값의 의도하지 않은 외란을 교정하고 보상하는 방법이다. 방법 (1) 및 방법 (5)는 요각의 값을 획득하는 것에 중점을 두고 있다. 교정 및 보상이 양호할수록, 방법 (1) 또는 방법 (5)로 획득된 요각의 값이 더 정확하다. 자력계 및 움직임 센서로부터 수신된 동시적인 측정치의 각각의 데이터 집합에 대해 방법 (1) 및/또는 방법 (5)가 수행될 수 있다. 자력계 및 움직임 센서로부터 수신되는 동시적인 측정치의 각각의 데이터 집합에 대해 방법 (2), 방법 (3) 및 방법 (4)가 또한 수행될 수 있지만, 각각의 데이터 집합에 대해 방법 (2), 방법 (3) 및 방법 (4) 중 하나, 일부 또는 전부를 수행하는 것이 필요한 것은 아니다. 변하는 외부 조건 또는 사용자의 요청에 따라, 동시적인 측정치의 데이터 집합에 대해 하나, 일부 또는 전부가 수행되거나 아무것도 수행되지 않을 수 있다.

틸트-보상된 요각을 계산하는 방법

틸트를 고려한 각도 정보와 함께 교정된 자력계 측정치를 사용하여 임의의 3-D 각위치(배향)에서 요각을 계산하는 방법이 제공되어 있다. 이 방법은 어떤 경우에 종래의 방법보다 더 높은 정확도를 달성하고 모든 조건에서 정확도가 조금도 더 나빠지지는 않는다.

예시적인 실시예에 따르면, 도 4는 롤각 및 피치각 측정치와 요각의 원시 추정치를 사용하여 틸트-보상된 요각을 계산하는 방법(300)의 블록도이다. 자력계 및 움직임 센서에 의해 수행되는 동시적인 측정은 3-D 교정된 자력계 측정치(310), 그리고 롤, 피치각 틸트 보정된 측정치 및 요각의 원시 추정치(320)를 이들 방법의 입력으로서 제공하는 것을 가능하게 해준다. 알고리즘(330)은 요각(340)의 값 및 요각(340)에 대한 추정된 오차(350)를 계산하고 출력한다. 틸트는 중력 기준계의 Z 축인 중력에 대한 몸체 기준계의 z 축의 경사이다. 틸트는 몸체의 선가속도를 중력과 비교함으로써 평가될 수 있다.

3-D 교정된 자력계 측정치(310)가 자력계 제조 특징, 강철 및 연철 효과, 정렬 및 동적 근방계를 고려하는 복수의 파라미터를 사용하여 자력계로부터 수신된 원시 신호로부터 획득된다. 이와 같이, 3-D 교정된 자력계 측정치는 몸체 기준계에서의 정적 국소 3-D 자계이다.

이하의 수학적 표현식은 지구 중심으로(아래쪽으로) 향해 있는 플러스 z-축과, 중력에 수직인 평면에, 각각, 자북과 동쪽으로 향해 있는 x-축 및 y-축을 갖는 것으로 정의되는 지구-고정 기준계 xyz를 나타낸다.

이하의 표 1은 방법(300)에 관계된 알고리즘을 설명하는 데 사용되는 표기법의 목록이다.

도 5를 고려하면, 지구-고정 중력 기준계를 현재의 장치의 몸체 기준계로 만드는 회전 행렬

은 3번의 회전을 포함하는 오일러각 시퀀스(Euler angle sequence)이고, 하기 수학식 1로 주어진다.

도 5에 예시된 바와 같이, 지구-고정 중력 기준계에서의 자계는 수학식 2로 표현될 수 있고,

여기서,

는 벡터

와 벡터

사이의 각도이고, 이는 수학식 3에 의해 복각

에 관계되어 있다.

3-D 교정된 자력계 측정치(310)는 수학식 4로 표현될 수 있고,

여기서,

는 수학식 5이며,

은

의 결합 확률 밀도 함수를 갖는 백색 가우시안 측정 잡음이다.

수학식 1 및 수학식 2를 수학식 5에 대입하면, 실제 자계(잡음이 없음)는 수학식 6이다.

이어서, 정규화된

은 수학식 7에 의해 주어진다.

정규화된

은 중력에 평행한 성분

과 중력에 수직인 성분

의 합이다.

유의할 점은 (1) 중력에 평행한 성분

가 요각

에 관한 정보를 전달하지 않고, (2) 각도

가

와 평행한 정규화된 성분의 마이너스

사이의 각도이다.

따라서, 보정된 입력 틸트각

및

이 주어지면,

은 이어서

을 계산하기 위해 보정된 자력계 입력

과 함께 사용된다.

추정된

을 사용하고 수학식 10 및 수학식 11을 수학식 7에 대입하면, 하기의 관계식이 얻어진다.

수학식 12에 기초하여, 요각을 계산하기 위해 종래의 방법과 상이한 3개의 방법이 여기에서 제안된다. 하기의 수학식들을 단순화시키기 위해, 수학식 13을 정의하기로 한다.

와

의 Z 성분의 곱으로부터

과

의 Y 성분의 곱을 감산함으로써, 수학식 14가 얻어진다.

이와 유사하게,

과

의 Z 성분의 곱을

과

의 Y 성분의 곱에 가산함으로써, 수학식 15가 얻어진다.

의 X 성분은 수학식 16이다.

요각

을 계산하는 제1 방법에서, 수학식 14가

과 곱해지고 수학식 15에 의해 나누어져 수학식 17을 얻는다.

요각

을 계산하는 제2 방법에서, 수학식 14가

과 곱해지고 수학식 16에 의해 나누어져 수학식 18을 얻는다.

요각

을 계산하는 제3 방법에서, 수학식 14 내지 수학식 16이 결합되어 수학식 19를 산출한다.

일 실시예에서, 이 알고리즘은 상기 3가지 방법 중 최종적인

에 대해 최고의 정확도를 갖는 방법을 동적으로 선택하는데, 그 이유는 3가지 방법에 대한 오차가 각각의 채널에 따른 자력계 잡음과 입력 롤각 및 피치각의 오차 둘 다의 상이한 함수이기 때문이고(어떤 방법은 어떤 오차 소스에 의해 더 영향을 받는 반면 다른 오차 소스에 의해 덜 영향을 받음), 예컨대, 방법 1은 자력계의 x-축 측정치의 오차에 영향을 받지 않고, 방법 2는

의 오차에 대한 함수이고, 따라서, 피치각이 0도에 가까울 때, 방법 2는 피치의 오차에 덜 민감하다). 일 실시예에서, 이 방법은 다음과 같이 동적으로 선택될 수 있다: (1) 피치각의 절대값이 0 내지 π/4인 경우, 제2 방법을 사용함; (2) 피치각의 절대값이 π/3 내지 π/2인 경우, 제1 방법을 사용함; (3) 그렇지 않은 경우, 제3 방법을 사용함. 이 방식에 의해 각각의 개별 영역에서 장치의 배향에 덜 민감한 보다 안정된 요각이 얻어진다. 유의할 점은, 이 동일한 기본 방식이 방정식들에서의 각각의 요소의 예상된 정확도에 기초하여 다양한 추정치를 병합하는 단일 방정식으로 구현될 수 있다는 것이다. 또한, 유의할 점은 이 동일한 방식이 자력계 측정치를 사용한 피치 및 롤의 계산에서 사용될 수 있다는 것이다.

참조를 위해, 종래의 방법은 하기의 수식을 사용하여

을 계산한다.

이 종래의 계산은 모든 오차 소스(즉, 롤각의 오차, 피치각의 오차, 3개의 축 각각에 대한 자력계 측정치의 오차)에 의해 무차별적으로 영향을 받는다. 일 실시예에서, 제1 방법, 제2 방법, 및 제3 방법 중 하나 이상 이외에 이 종래의 방법이 사용될 수 있다.

따라서, 제1 방법, 제2 방법 및 제3 방법 중 요각의 최상의 추정치를 사용하여 달성되는 정확도(가장 작은 추정된 오차를 가짐)가 종래의 방식보다 우수하다.

자세-독립적 파라미터를 교정하는 방법

일부 실시예에 따르면, 3축 자력계의 자세-독립적 파라미터(스케일, 비직교성/스큐/교차 결합, 오프셋)를 교정하는 방법이 제공된다. 이들 자세-독립적 파라미터는, 발산 문제 또는 국소 최소값으로의 수렴이 염려되지 않도록, 수학적 닫힌 형태에서의 분석해(analytical solution)로서 동시에 얻어진다. 게다가, 반복 계산이 필요하지 않으면서, 이 방법이 실시간으로 수행될 수 있다. 동일하거나 상이한 배향에서 자력계로부터의 다른 측정치에 대해 교정이 반복될 필요가 있는지 또는 현재의 파라미터 값이 원하는 정확도 기준을 만족시키는지를 판정하기 위해 파라미터의 추정 정확도가 사용될 수 있다.

도 6은 예시적인 실시예에 따른, 자세-독립적 파라미터를 교정하는 방법(400)의 블록도이다. 방법(400)은, 입력(410)으로서, 3-D 자력계로부터의 원시 측정치를 가진다. 이 입력을 사용하여, 알고리즘(420)은 일련의 자세-독립적 파라미터(430) 및 이들 자세-독립적 파라미터(430)를 사용하여 계산되는 현재 측정된 3-D 자계(440)의 값을 출력한다.

자세-독립적 파라미터를 교정하는 데 사용될 데이터를 수집하는 데 사용되는 시스템(500)이 도 7에 예시되어 있다. 시스템(500)은 4개의 블록 - 감지 요소(510), 데이터 수집 엔진(520), 파라미터 결정 유닛(530) 및 정확도 추정 유닛(540) - 으로 이루어져 있다.

센서 요소(510)는 감지된 자계값을 나타내는 잡음이 많고 왜곡된 신호를 출력한다. 데이터 수집 블록(520)은 센서 데이터를 샘플별로 축적함으로써 파라미터 결정을 준비한다. 파라미터 결정 유닛(530)은 센서 요소를 교정하여 일정한 진폭의 측정치를 제공하기 위해 자세-독립적 파라미터를 계산한다. 정확도 추정 유닛(540)은 계산된 자세-독립적 파라미터의 오차를 계산하고, 이는 사전 결정된 원하는 정확도가 달성되었는지를 나타낸다.

이하의 표 2는 자세-독립적 파라미터를 교정하는 방법에 관계된 알고리즘을 설명하는 데 사용되는 표기법의 목록이다.

자력계의 감지 요소에 의해 검출되는 신호는 그 근방에 강자성 원소가 존재하는 것에 의해 왜곡된다. 예를 들어, 자계와 둘러싸고 있는 설치 재료 사이의 간섭에 의해, 국소 영구 자화된 재료에 의해, 센서 자체의 스케일링, 교차 결합, 바이어스에 의해, 그리고 센서의 기술적 한계 등에 의해 신호가 왜곡된다. 자기 왜곡 및 감지 오차의 유형 및 효과는 W. Denne, Magnetic Compass Deviation and Correction, 3rd ed. Sheridan House Inc, 1979. 등의 많은 공개적으로 이용가능한 참조 문헌에 기술되어 있다.

3축 자력계 판독치(즉, 3-D 측정된 자계)가 참조 문헌 J.F. Vasconcelos 등의 “A Geometric Approach to Strapdown Magnetometer Calibration in Sensor Frame”에서 수학식 21로서 모델링되었다.

참조 문헌 R. Alonso 및 M. D. Shuster의 “Complete linear attitude-independent magnetometer calibration ” in The Journal of the Astronautical Sciences, 50(4):477-490, October-December 2002로부터의 보다 실제적인 수식은 일반성을 잃지 않고 수학식 22이고,

여기서 D는 센서 기여분 및 연철 효과 둘다로부터의 스케일링 및 스큐를 결합시킨 것이고, O는 지구-고정 중력 기준계에 대한 연철 효과 및 센서의 내부 정렬 오차 둘 다를 결합시킨 오정렬 행렬이며, b는 강철 효과 및 센서의 고유 기여분 둘 다로 인한 바이어스이고, n은 영 평균 및 일정한 표준 편차 σ를 갖는 변환된 센서 측정 잡음 벡터이다.

O 및

둘 다가 벡터의 방향만을 변경시키기 때문에,

의 크기는 지구-고정 몸체 기준계에 대한 자력계의 배향의 상수 불변량이다. 점

가 구(sphere)로 제약되는 경우, 자력계 판독치 B_k는 타원체 상에 있다.

임의의 B_k 집합(즉, 타원체의 임의의 부분)에 대해, 수학적 닫힌 형태에 의해 D와 b를 동시에 분석적으로 구하는 방법이 제공된다. 수학식 22는 수학식 23으로 바꾸어 쓸 수 있다.

수학식 23의 양변에서의 크기 제곱도 또한 같고, 수학식 24를 산출한다.

이기 때문에, 수학식 24가 수학식 25로 바꾸어 쓸 수 있다.

수학식 25의 우변이 잡음 항인 경우, 수학식 25에 대한 해는 수학식 26과 같이

내지

의 최소 제곱 근사(least square fit)일 수 있다.

그렇지만, 수학식 26이 D와 b의 고비선형 함수(highly nonlinear function)이기 때문에, 간단한 선형 분석해가 없다.

수학식 25에서의 잡음을 무시한 정의 - 수학식 27 및 수학식 28 - 를 사용하고,

수학식 29를 전개함으로써,

하기의 관계식이 얻어진다.

수학식 30을 간략화하기 위해, Q 원소가 수학식 31과 같이 정의된다.

그러면, 수학식 28에 기초하여, E는 수학식 32가 된다.

행렬 pD는 SVD(singular value decomposition, 특이값 분해)법을 사용하여 구해질 수 있고,

여기서, s는 3x3 대각 행렬이다. 이어서 S의 각각의 원소에 제곱근을 적용하면, 다음과 같이 또 하나의 3x3 대각 행렬 w가 얻어지고, 이어서 pD가 얻어진다.

오프셋 b는 수학식 36과 같이 계산된다.

Q를 구하기 위해, 3개의 크기 Q(1), Q(2), 및 Q(3)에 걸친 평균이 수학식 37과 같이 정의된다.

새로운 파라미터 벡터 K를 사용하면,

수학식 29는 수학식 39로 된다.

다음과 같이 Nx9 행렬 T 및 Nx1 벡터 U를 정의하자.

이 표기법을 사용하면, N개의 샘플 측정치에 대해, 수학식 39는 수학식 42로 되고,

수학식 43에 의해 해가 구해질 수 있다.

이어서, 수학식 38 및 수학식 32로부터, E는 수학식 44와 같이 쓰여질 수 있다.

수학식 45를 정의하자.

G는 수학식 33 내지 수학식 35를 사용하여 pD와 동일한 방식으로 구해지고,

b는 수학식 36, 수학식 38 및 수학식 46을 결합함으로써 계산된다.

수학식 38에서의 K(9)의 정의 및 수학식 47을 수학식 31에 대입하면, co는 다음과 같이 계산된다.

마지막으로, 수학식 48을 수학식 46 및 수학식 47에, 이어서 수학식 27에 대입하면, D 및 b가 완전히 구해진다.

는 국소 지자계 강도의 제곱을 가리킬 수 있다. 심지어 강도가 미지의 값을 가지며, 임의의 임의적인 상수로 사전 설정될 수 있고, 해에 대한 유일한 차이는 3개의 축 모두의 계산된 9개 요소(3 스케일, 3 스큐, 및 3 오프셋) 모두에 대한 상수 스케일 차이이다.

앞서 설명한 형식주의에 기초하여, 예시적인 실시간 구현예에서, 각각의 시간 스텝에 대해, 데이터 수집 엔진(520)은 2개의 변수 행렬을 저장하고: covPInvAccum_이라고 하는 하나의 9x9 행렬은

를 누적하는 데 사용되고, zAccum_이라고 하는 다른 변수 9x1 행렬은

를 누적하는 데 사용된다. 시간 스텝 n+1에서, 다음과 같은 수학식들에 따라 행렬이 갱신된다.

T의 n+1 행에 있는 원소인

, 및 U의 n+1 행에 있는 원소인

은 현재의 시간 스텝에서의 자력계 샘플 측정치만의 함수이다. 이어서, 수학식 43에 기초하여, K가 구해지고, 이어서 수학식 33 내지 수학식 35를 사용하여 G가 구해진다. 임시 변수

가 수학식 51과 같이 계산된다.

수학식 45의 대입과 함께 이

를 수학식 48에 집어넣음으로써, co가 얻어진다.

그에 부가하여, 수학식 51이 수학식 47에 대입되고, 계산된 co가 수학식 46 및 수학식 47에 적용되며, 이어서 수학식 27을 사용하여, D 및 b(즉, 전체 교정 파라미터 집합)가 얻어진다.

이하의 알고리즘이 D 및 b를 구하는 정확도를 결정하는 데 적용될 수 있다. K의 추정치의 오차 공분산 행렬이 수학식 52에 의해 주어지고,

여기서,

이다.

구해진 파라미터에 대한 K의 야코비 행렬

은 다음과 같이 주어진다.

이와 같이, J의 추정치의 오차 공분산 행렬이 수학식 58에 의해 주어진다.

추정치 J의 오차는 수학식 59이다.

앞서 상세히 기술한 형식주의에 따른 자세-독립적 파라미터를 교정하는 방법이 지구-고정 기준계에서의 상수 물리량 벡터를 측정하는 임의의 센서(지구 중력을 측정하는 가속도계 등)를 교정하는 데 적용될 수 있다. 이들 방법은 임의의 타원체를 구로 근사화시키기 위해 전체 파라미터 집합을 계산하는 데 적용될 수 있고, 여기서 타원체는 원점으로부터 오프셋될 수 있고 및/또는 스큐되어 있을 수 있다. 각각의 샘플 측정에 대해

가 알려져 있는 한, 이들 방법이 동적 시변

에도 사용될 수 있다.

co를 정의하는 방식이 수학식 37과 상이할 수 있다 - 예를 들어, Q(1), Q(2) 및 Q(3)의 다른 선형 결합이 유사하거나 심지어 더 나은 결과를 가져온다 -. 이러한 선형 결합의 일반 형태는 수학식 60이고:

여기서 그 계수들의 합은 1이다. 즉,

이다.

수학식 40 및 수학식 41은 상이한 샘플에서의 측정 잡음을 고려하는 것으로 확장될 수 있고, 확장된 수학식은 잡음 분산의 역을 가중치로서 사용한다.

측정 오차의 다른 기능은 또한 유사한 방식으로 T 및 U에 대한 가중치로서 역할할 수 있다.

종래의 비선형 최소 제곱 근사 방법은 해가 발산하거나 전역 최소값 대신에 국소 최소값으로 수렴할 수 있다는 단점을 가지며, 그로써 종래의 비선형 최소 제곱 근사 방식은 반복을 필요로 한다. 종래의 교정 방법 중 어느 것도 완전 분석적 닫힌 형태로 D 및 b를 구하지 않는다. 예를 들어, 한가지 종래의 방법은 스큐를 고려하지 안하고 스케일만을 결정한다(즉, 총 9개의 요소 중 6개만이 스큐가 0이라는 가정에 기초하여 결정된다).

자세-의존적 자력계-정렬 파라미터를 교정하는 방법

자계, 특히 복각(즉, 국소 지자계의 중력에 수직인 평면으로부터의 이탈)에 관한 사전 지식 없이 3-D 자력계를 지구-고정 중력 기준계에 맞춰 정렬하고 지구-고정 중력 기준계에 대한 동시적으로 측정된 각위치의 시퀀스에서 미지의 상수 초기 요각 오프셋을 가능하게 해주는 방법이 제공된다. 연철 효과로부터 얻어지는 등가의 오정렬 효과가 또한 동일한 방식으로 해결된다. 정렬 알고리즘 동적 특성을 제어하기 위해 정렬 정확도에 대한 검증 방법이 보강된다. 교정 및 검증의 결합은 알고리즘이 충분히 안정된 채로 있으면서 보다 빠르게 수렴하게 한다. 이는 또한 실시간 구현이 신뢰성있고 강력하며 간단할 수 있게 해준다.

도 8은 예시적인 실시예에 따른, 지구-고정 중력 기준에 맞춰 3-D 자력계를 정렬하는(즉, 자세-의존적 파라미터를 교정하는) 방법(600)의 블록도이다. 방법(600)은 자력계를 사용하여 측정되고 교정된 자세-독립적 파라미터를 사용하여 계산된 자계(610), 및 미지의 초기 요 오프셋의 영향을 받는 각위치(620)를 입력으로서 가진다. 이들 입력을 사용하여, 센서 정렬을 위한 알고리즘(630)은 장치의 몸체 기준계에 대한 3-D 자력계의 정렬 행렬(640)을 출력하고, 그의 사용은 측정된 자계의 완전히 교정된 값(650)을 계산하는 것을 가능하게 해준다.

도 9는 다른 예시적인 실시예에 따른, 9축 시스템에서 3-D 자력계를 정렬하는 방법(700)의 다른 블록도이다. 도 9의 블록도는 데이터 흐름을 강조한 것이다. 9축 시스템(710)은 3-D 자력계, 3-D 가속도계 및 3-D 회전 센서 - 그의 감지 신호가 센서 해석 블록(720)으로 송신됨 - 를 포함한다. 센서가 장치의 자계, 선가속도, 및 각속도에 대응하는 잡음이 많고 왜곡된 감지 신호를 제공한다. 센서 해석 블록(720)은 (1) 정렬을 보정하지 않고 자력계 측정치로부터 스케일, 스큐, 및 오프셋을 제거하기 위해, (2) 스케일, 스큐, 오프셋 및 가속도계에 대한 비선형성을 제거하기 위해, (3) 스케일, 스큐, 오프셋, 및 회전 센서에 대한 선가속도 효과를 제거하기 위해, 그리고 (4) 가속도계 및 회전 센서를 몸체 기준계에 맞춰 정렬하기 위해, 사전 계산된 파라미터(자세-독립적 파라미터 등)를 사용하여 감지 신호를 표준화된 단위로 변환한다. 가속도계 및 회전 센서의 그 해석된 신호는 이어서 미지의 초기 요각 오프셋을 제외한 장치의 자세(즉, 지구-고정 중력 기준계에 대한 각위치)의 추정치를 발생하기 위해 (예컨대, Liberty 특허에 기술된 방법 또는 다른 방법을 사용하여) 각위치 추정 알고리즘(730)에 의해 사용된다. 시간 시퀀스에서의 추정된 자세 및 자계의 자세-독립적 교정된 값이 자력계 정렬 추정 알고리즘(740)에 입력된다. 그 다음에, 추정된 초기 요각 오프셋 및 경사각이 자력계 샘플과 함께 이어서 정확도를 평가하는 정렬 검증 알고리즘(750)에 입력된다. 정렬 검증 알고리즘(750)은 정렬 추정 알고리즘(740)이 충분히 잘 기능했는지에 관한 신뢰할 수 있는 표시를 제공한다.

이하의 표 3은 자세-의존적 파라미터를 교정하는 방법에 관계된 알고리즘을 설명하는 데 사용되는 표기법의 목록이다.

정렬 오차의 주요 원인은 장치에 대한 자력계의 불완전 설치(즉, 장치의 몸체 기준계에 대한 오정렬), 및 연철 효과로부터의 영향이다. 시간 스텝 t_n에서의 자세 독립적 교정된 자력계 측정치는 수학식 64이고,

여기서

은 수학식 65로 분해될 수 있다.

은 자력계의 측정치와 장치의 몸체 기준계 사이의 오정렬 행렬이고,

은 시간 스텝 t_n에서의 지구-고정 좌표계에 대한 정확한 각위치이다. 3축 가속도계 및 3축 회전 센서를 사용한

의 최상의 추정치는

로 나타내어진다. 이 추정치는 초기 요각 오프셋을 제외한 짧은 기간에서 높은 정확도를 가진다.

는 수학식 67로서 표현될 수 있다.

제한 없이, 자북이 지구-고정 중력 기준계의 플러스 X 축으로서 사용된다. 수학식 65 내지 수학식 67을 수학식 64에 대입하면, 하기의 수학식이 얻어진다.

그러면, 문제는

및

의 행렬이 주어진 경우

및

을 추정하는 것으로 된다. 간략함을 위해,

을 A로 표시하고 C를 수학식 70으로서 정의한다.

확장된 Kalman 필터(EKF) 구조체의 6개의 원소는 수학식 71과 같고,

여기서,

는 벡터 회전을 나타내는 4원수의 스케일 및 벡터 원소이고,

는 국소 자계의 경사각이며,

는 기준계의 각위치에서의 초기 요각 오프셋이다.

X 및

의 초기값은 다음과 같다.

상태에 대한 과정 모델은 정적이다 - 즉,

임 -. 측정 모델은 다음과 같다.

예측된 측정치가 수학식 75에 의해 주어진다.

상태 X에서의 4원수와 정렬 행렬

사이의 관계식이 수학식 76에 의해 주어진다.

에 대한 A의 편미분은 다음과 같이 주어진다.

및

에 대한 C의 편미분은 다음과 같다.

G는 다음과 같이 정의된다.

원소들이 X에 대한 h의 편미분인 야코비 행렬은 다음과 같다.

상태 및 그의 오차 공분산 행렬 갱신에 대해 다음과 같이 표준 EKF 계산 절차가 사용된다.

(1) 오차 공분산 예측

(2)혁신 계산

수학식 75를 수학식 86에 대입하면, 수학식 87이 얻어진다.

(3) Kalman 이득 계산

여기서, R은 다음과 같이 주어지는 자력계 측정 잡음 공분산이고:

(4) 상태 보정

(5) 오차 공분산 보정

EKF의 표준 절차 이외에, 이 방법은 상태를 제한된 채로 유지하기 위해 2개의 단계를 더 실행하며, 이는 재귀적 필터를 안정화시켜 필터가 발산하는 것을 방지한다.

(6) 4원수 정규화, 회전 행렬을 나타내는 유효한 4원수가 1의 진폭을 가진다.

(7) 경사각 및 초기 요각 오프셋에 대한 위상 랩(phase wrap), 유효한 경사각은

와

사이로 제한되어 있고, 유효한 요각은

와

사이로 제한되어 있다. 첫째, 경사각 추정치는, 예를 들어, 수학식 93을 사용하여

내로 제한되고,

여기서,

함수는 다음과 같은 일을 한다.

코드 1

둘째, 경사각 추정치는 또한

내로 제한되는데, 그 이유는 이 동작이 코사인 및 사인의 부호를 변경하기 때문이며, 초기 요각 오프셋에 대한 적절한 변화가 수반될 필요가 있고, 예시적인 코드는 다음과 같다.

코드 2

마지막으로, 초기 요각 오프셋 추정치가

내로 제한된다.

단계 6 및 단계 7이 필요하고 중요하지만, 필터를 안정적으로 유지하는 데 이들 단계로 충분한 것은 아니며 이들 단계가 필터를 보다 빠르게 수렴시키지 않는다.

이 방법에 부가되는 다른 제어 인자는 동적 Q 조정이다. 종래의 방법에서는, Q=0인데, 그 이유는 추정의 상태가 시간에 따라 일정하기 때문이다. 그렇지만, 이것은 데이터 시퀀스가 그다지 익숙하지 않을 때 아주 느린 수렴 속도를 야기한다. 예를 들어, 초기에 수집된 모든 데이터점이 오랫 동안 각위치의 아주 작은 이웃으로부터 온 것인 경우, 이는 궁극적으로 P를 아주 작게 만들 수 있는데, 그 이유는 각각의 시간 스텝이 P를 약간 더 작게 만들기 때문이다. 이어서 추가의 데이터점이 아주 다양한 각위치로부터 아주 짧은 시간 시스템에서 수집될 때, 필터는 아주 작은 P로 인해 그의 상태를 참값으로 신속히 갱신할 수 없다.

이 방법은 영이 아닌 Q를 가능하게 해주고, 이는 필터가 시스템 상태를 적절한 속도로 갱신할 수 있게 해준다. 일반적으로, P가 아주 크게 되어 필터를 불안정하게 만들도록 P를 증가시킬 위험이 존재하지만, 이 방법은 Q를 동적으로 조정하고 따라서 빠르게 수렴하는 이점을 가지며 또한 충분히 안정되도록 해줄 수 있다. 이를 위해, 상수 기준 Q₀는 필터가 전체 동적 범위에 대해 할 수 있고 변수가 각각의 시간 스텝에 대해 취할 수 있는 최대 변화로 설정된다.

이 방법에서 각각의 시간 스텝에서 최종적인 Q를 조정하기 위해 2개의 동적-변화 곱셈 인자가 사용된다.

은 현재의 시스템 상태와 정확도 검증 알고리즘으로부터 획득된 시스템 상태 사이의 추정된 오정렬 각도의 차이의 함수이도록 설계된다. 이 차이가 충분히 클 때,

은 필터가 그의 최대 수렴 속도로 실행할 수 있게 해준다. 이 차이가 원하는 정확도와 비교하여 충분히 작을 때,

은 필터가 느려지게 하고 미세-조정을 수행한다. 예시적인 실시예에서, 이 관계식은 각각의 시간 스텝에서 다음과 같이 구현된다.

코드 3

여기서,

는 음이 아닌 상수이고 1보다 훨씬 더 작다.

는 감쇠 인자(decay factor)이다. 각위치가 고정된 각위치의 이웃에 있을 때,

는 지수적으로 감쇠된다. 각위치가 사전 정의된 임계값 ANGLE_TOL보다 더 많이 변할 때,

는 다시 1로 점프한다. 이것을 함으로써, 장치가 아주 좁은 각위치 공간 내에 머물러 있을 때 필터가 훨씬 더 큰 P를 갖지 않게 된다. 따라서, 안정성이 보장된다. 2개의 각위치 사이의 차이가 다음과 같이 주어진다.

코드 4

여기서 A 및 Aold는, 각각, 2개의 4원수의 방향-코사인 행렬 표현이고, q = dcm2q(dcm)는 방향-코사인 행렬을 4원수 표현으로 변환하는 함수이며, [v, phi] = qdecomp(q)는 단위 벡터 및 4원수의 회전각 성분을 분해하는 함수이다.

계산의 예시적인 구현이 다음과 같이 주어진다.

코드 5

DECAY_FACTOR는, 예를 들어, 0.95로 설정될 수 있다.

상태가 가장 최근의 측정치로 갱신될 때, 추정된 경사각 및 초기 요각 오프셋이 수학식 97의 최상의 시퀀스를 구성하는 데 사용된다.

및

의 시퀀스 쌍이 주어진 경우,

을 푸는 것은 Wahba 문제라고 알려진 것으로 된다. 이 문제를 해결하기 위해 많은 대안의 알고리즘이 개발되었다. 이하에서 단계 1 내지 단계 4로서 기술되어 있는 Landis Markley의 SVD(Singular Value Decomposition) 알고리즘이 여기서 사용된다.

(1) 3x3 행렬 L을 구성한다.

(2) 특이값 분해(SVD)를 사용하여 L을 구성한다.

(3) 부호를 계산하고

를 구성한다.

(4) A를 계산한다.

A가 계산될 때, 이 방법은 이 A를 상기 EKF의 가장 최근의 상태에서 얻어진 것과 비교하고, 차이의 각도가 코드 4를 사용하여 계산된다. 차이의 각도는 추정된 정렬 행렬의 정확도의 추정치이다. 앞서 언급한 바와 같이, 설계된 EKF에서의 동적 Q 조정에서

의 곱셈 인자를 결정하기 위해 차이의 각도가 또한 피드백된다.

보다 쉬운 실시간 구현을 위해, 이력 데이터를 다음과 같이 재귀적으로 저장하기 위해 9개의 1x3 영속 벡터 변수(persistent vector variable)가 사용된다.

따라서, 수학식 98이 수학식 103을 사용하여 계산될 수 있다.

각위치의 참조된 시퀀스는 임의의 다른 움직임 센서의 결합으로부터, 심지어 다른 자력계로부터 온 것일 수 있다. 이 방법이 3-D 가속도계 및 3-D 회전 센서를 갖는 9축 유형의 센서 유닛인 다른 센서 유닛에 대해 사용될 수 있다. 각위치의 참조된 시퀀스가 다양한 센서-융합 알고리즘을 사용하여 획득될 수 있다.

중력 기준계의 축이 중력 및 자북 방향을 사용하여 위치 확인될 수 있는 한, 지구-고정 중력 기준계가, 중력 및 자북 대신에, 다른 방향을 x-축 및 z-축으로서 갖는 것으로 정의될 수 있다.

참조된 각위치가 미지의 초기 요 오프셋을 갖지 않는 경우,

는 참조된 지구-고정 좌표계에 대한 국소 자계의 요각일 수 있고, 수학식 67을 수학식 104로서 다시 쓸 수 있다.

이러한 정렬 행렬이 획득된 후에, 국소 자계 벡터가 또한 지구-고정 좌표계에서 자동으로 구해지는데, 그 이유는

및

가 EKF 상태에서 동시에 구해지기 때문이다.

정렬 알고리즘이 임의의 참조된 장치 몸체에 의한 임의의 센서 3D 정렬을 위해 사용될 수 있고, 자력계 또는 관성 몸체 센서로 제한되지 않는다.

정렬 알고리즘은 하나의 단계에서 이를 풀기 위해 일련의 데이터를 한꺼번에 받을 수 있다.

이 방법은 정확도 검증 알고리즘에 대해 앞서 기술한 것 대신에 Wahba 문제를 풀기 위해 다른 알고리즘을 이용할 수 있다.

그에 부가하여, 각도 차이가 일치성(coincidence)(즉, 해가 향상될 수 없는 동안 루프하는 것)을 피하기 위해 다수의 반복에 대해 사전 결정된 허용 범위보다 작도록 하기 위해 안정성 카운터가 사용될 수 있다.

유사한 결과를 달성하기 위해 EKF의 다른 초기화가 사용될 수 있다. 정렬 추정 알고리즘이 초기화에 민감하지 않다.

상기 예시적인 실시예에서 사용되는 상수가 특정의 목적을 달성하도록 조정될 수 있다. k₁ 및 k₂ 값과 그의 적응적 변화 거동이 환경, 센서 및 응용 등에 따라 예시적인 실시예와 상이할 수 있다.

요약하면, 이 섹션에서 기술되는 방법은, 모든 다른 파라미터(스케일, 스큐 및 오프셋)가 이미 사전 교정되어 있거나 다른 방식으로 충분한 정확도로 알려져 있는 한, 임의의 미지의 환경, 미지의 경사각, 및 참조된 자세에서의 미지의 초기 요각 오프셋(총 5개의 독립 변수)에서 참조된 장치의 몸체-고정 기준계에 대한 자력계의 오정렬을 실시간으로 추정하는 간단하고 빠르며 안정적인 방식을 제공한다. 이들 방법은 지구-고정 중력 기준계에서의 국소 자계의 사전 지식을 필요로 하지 않는다. 실시간의 신뢰할 수 있고 강력하며 친숙한 동작을 가능하게 해주기 위해 정렬 정확도에 대한 검증 방법이 정렬 알고리즘과 연관되어 있다.

근방계를 추적하고 보상하는 방법

지구-고정 중력 기준계에 대한 자력계의 3-D 각위치 추정치를 사용하여 자력계 측정으로부터 동적 근방 자계를 동적으로 추적하고 보상하는 방법이 제공된다. 3-D 각위치가 완벽하게 정확하지 않으며, 롤각, 피치각, 및 적어도 요각 드리프트에서 오차를 포함할 수 있다. 동적 근방계에 대해 보상된 자계 측정이 나침반 또는 3-D 각위치 결정에 유용하다. 유사한 결과를 달성할 수 있는 종래의 방법이 없다.

예시적인 실시예에 따르면, 도 10은 예시적인 실시예에 따른, 동적 근방 자계를 추적하고 보상하는 방법(800)의 블록도이다. 자력계를 완전히 교정한 후에 계산되는 측정된 자계값(810) 및 몸체 센서에 의한 동시적인 측정치로부터 추론되는 기준 각위치(820)이 동적 근방 자계를 추적하고 보상하는 알고리즘(830)에 입력된다. 알고리즘(830)을 적용한 결과는 정적 국소 3-D 자계값(840)(즉, 교정되고 근방계 보상된 자력계 측정치) 및 정적 국소 3-D 자계값(840)과 연관된 오차 추정치(850)이다.

도 11은 예시적인 실시예에 따른, 근방 자계를 추적하고 보상하는 방법(900)의 블록도이다. 도 11의 블록도는 데이터 흐름을 강조한 것이다. 3-D 자력계를 포함하는 센서 블록(910)은 센서 해석 블록(920)에 감지 신호를 제공한다. 센서 해석 블록(920)은 사전 계산된 파라미터를 사용하여, 왜곡된 센서 신호를 향상시켜 표준화된 단위로 변환하며, 스케일, 스큐, 오프셋 및 오정렬을 제거한다. 자계값이 동적 근방 자계 추적 및 보상 알고리즘(930)으로 출력된다. 지구-고정 중력 기준계에 대한 장치의 각위치(940)가 또한 알고리즘(930)에 입력된다. 각위치는 랜덤한 롤 및 피치각 오차에, 특히 랜덤한 요각 오차 드리프트에 영향을 받는다. 알고리즘(930)은 동적 근방 자계로 인한 변화를 추적하고, 장치의 몸체 기준계에서의 입력 자계값을 보상한다. 알고리즘(930)은 또한 보상된 자계 측정치를 사용하여, 입력된 각위치, 특히 요각 드리프트에서의 오차를 보정한다.

이하의 표 4는 근방계를 추적하고 보상하는 방법에 관계된 알고리즘을 설명하는 데 사용되는 표기법의 목록이다.

지구-고정 중력 기준계에서의 자계가 일정할 때, 장치의 몸체 기준계에서의 자력계에 의해 측정된 자계는 지구-고정 중력 기준계에 대한 장치의 몸체 기준계의 3-D 배향(각위치)을 결정하는 데 사용될 수 있다. 그렇지만, 지구-고정 중력 기준계에서의 자계가 시간에 따라 변할 때, 자력계 측정치가 상당히 변화된다. 이러한 시간 의존적 변화는 강철 효과 또는 연철 효과 등의 소스를 부가하거나 제거하는 이어폰, 스피커, 휴대폰 등의 임의의 근방계 외란으로 인한 것일 수 있다.

자력계가 배향 추정을 위해 또는 나침반용으로 사용될 때 근방계 외란의 존재가 알려져 있지 않은 경우, 추정된 배향 또는 북쪽 방향이 부정확하다. 따라서, 3-D 배향 및 나침반을 결정하기 위해 자력계 측정치를 실제로 사용하기 위해, 근방 자계 추적 및 보상이 바람직하다. 게다가, 3-D 가속도계 및 3-D 회전 센서를 포함하는 결합으로부터 획득된 각위치가 요각 드리프트 문제에 의해 영향을 받는데, 그 이유는 지구-고정 중력 기준계에 대한 장치의 몸체 기준계의 절대 요각의 직접적인 관측이 없기 때문이다. 근방계에 대해 보상되는 자계값은 이 단점을 보정하여, 요각 드리프트 문제를 치유한다.

교정된 자력계(연철 및 강철 효과 교정을 포함함)는 다음을 측정하고:

여기서,

이고

이다.

이 방법은

를 동적으로 추적하고 이를 사용하여

를 추정하며, 이어서

으로부터 이를 보상하여

를 획득하고, 추정된

는 3-D 배향 측정 및 나침반용으로 사용될 준비가 되어 있다. 방법은 다음과 같은 단계들을 포함할 수 있다.

단계 1: 2개의 영속적 3x1 벡터에서, 동적

의 추정치 및 가장 최근의 정상

의 추정치 - 각각,

및

으로 표시되어 있음 - 를 저장한다.

단계 2: 지구-고정 중력 기준계에서의 가상 상수 3x1 벡터를 구성한다.

단계 3: 지구-고정 중력 기준계에서의 관측 벡터를 구성한다.

각각의 시간 스텝에 대해 하기의 단계들이 실행된다.

단계 4: 기준 배향(각위치)를 사용하여 장치의 몸체 기준계에서의

의 표현식을 계산한다.

수학식 108에 나타낸 방식으로

를 구성함으로써,

이

에서의 요각 오차에 의해 영향을 받지 않는다.

의 z 축의 값이

에 대한 벡터

의 상대 가중치를 나타내는

의 임의의 함수로 설정될 수 있다.

단계 5:

와

사이의 각도

를 계산한다.

단계 6: 지구-고정 중력 기준계에서의 자계(근방계를 포함함)를 예측한다.

단계 7: 현재의 자계 추정치와

사이의 차이를 계산한다.

단계 8: 예를 들어, 단일 지수적 평탄 필터를 사용하여 현재의 자계 추정치를 갱신한다.

단계 9:

의 총 크기를 계산하고, 그것과

의 크기 사이의 차이를 취한다.

단계 10:

와

사이의 각도

를 계산한다.

단계 11:

와

사이의 각도차를 계산한다.

단계 12: 예를 들어, 하기의 예시적인 실시예를 사용하여 근방 자계가 안정적인지를 평가한다.

코드 6

여기서

의 영속적 변수가 근방 자계가 얼마나 오랫동안 변하지 않는지를 기록하는 데 사용된다. 예시적으로,

이 3으로 설정될 수 있고,

가 4로 설정될 수 있다.

가 수학식 116에 의해 주어진다.

단계 13:

가 사전 정의된 임계값보다 클 때

을

으로 갱신하고(예컨대, 임계값이 1초와 동등하게 설정될 수 있음) 이어서

를 0으로 리셋한다. 단계 13의 예시적인 실시예는 하기의 코드이다.

코드 7

단계 14: 예를 들어, 하기의 서브단계를 수행함으로써, 현재의 샘플이 가장 최근의 추정된 정상 자계와 부합하는지를 평가한다.

서브단계 14.1:

와

사이의 각도차를 계산한다.

서브단계 14.2:

의 총 크기를 계산하고, 그것과

의 크기 사이의 차이를 취한다.

서브단계 14.3: 예를 들어, 하기의 코드를 사용하여 14.1 및 14.2에서 계산된 차를 사전 정의된 임계값과 비교한다.

예, 현재 샘플이 추정된 정상 근방 자계에 있고,

단계 15 및 단계 16으로 간다.

else

아니오, 단계 15 및 단계 16을 건너뛰고, 현재 샘플이 근방계 보상되지 않고, 배향 추정치 또는 나침반에 대해 주의를 기울일 필요가 있으며, 다음 샘플이 올 때까지 기다린다.

end

코드 8

여기서,

및

는 보다 많은 샘플이 포함될 수 있게 해주기 위해 타당할 정도로 크도록 설정될 수 있다. 유의할 점은, 코드 8에서의 “else” 단계에 대한 한가지 옵션이 현재의 자계를 더 잘 반영하도록 현재의 모델을 갱신하는 것이라는 것이다.

단계 15: 단계 14의 결과가 현재의 샘플이 가장 최근의 추정된 정상 자계와 부합한다는 것인 경우, 하기의 서브단계를 수행한다.

서브단계 15.1:

를 사용하여 지구-고정 중력 기준계에서의 벡터 관측을 구성한다.

서브단계 15.2: 장치의 몸체 기준계에서의 벡터 관측을 구성한다.

서브단계 15.3: 장치의 몸체 기준계 및 지구-고정 중력 기준계 둘 다에서의 벡터 관측으로 3x3 행렬을 형성한다.

서브단계 15.4: 보정된

의 해를 구한다. 이 서브단계가 각종의 상이한 알고리즘을 사용하여 구현될 수 있다. 특이값 분해(SVD)법을 사용한 예시적인 실시예가 이하에 기술되어 있다.

(1) SVD를 사용하여 G를 분해한다.

(2) 부호를 계산하고

를 구성한다.

(3)

를 계산한다.

단계 16: 근방 자계가 보상되어 있는

를 계산한다.

단계 17:

를 사용하여 요각 결정과 연관된 오차를 추정한다.

파라미터

및

가 자력계의 교정의 정확도의 동적 함수로 설정될 수 있다.

최상의 요각 추정치를 얻기 위해 상이한 요각 추정치들을 융합하는 방법.

요각의 잡음있는 추정치들을 융합(즉, 결합)하는 방법이 제공된다. 9축 유형의 장치에서, 하나의 요각 추정치가 교정된 자력계를 사용하여 획득될 수 있고, 다른 단기적으로 안정적이지만 장기적으로 드리프트하는 요각 추정치가 3-D 회전 센서(예컨대, 자이로스코프) 등의 움직임 센서로부터 획득될 수 있다. 이 방법은 요각 오차가 작을 때 평탄한 작은 조정을 가능하게 해주고 요각 오차가 클 때 신속한 큰 조정을 가능하게 해준다. 이하에서 기술되는 방법은 오차가 작을 때는 평탄하게 안정적인 값을 산출하는 요각에 대해 높은 정확도를 달성하면서, 오차가 클 때는 그에 응답하여 빠른 조정을 달성한다. 유의할 점은, 이 동일한 방식이 다른 배향 및 위치 파라미터에도, 특히 피치 및 롤각에 적용될 수 있다는 것이다.

예시적인 실시예에 따르면, 도 12는 최상의 요각 추정치를 얻기 위해 요각 추정치들을 융합시키는 방법(1000)의 블록도이다. 3-D 교정된 자력계로부터의 요각 추정치(1010) 및 몸체 센서(들)로부터의 요각 측정치(1020)가 융합 알고리즘(1030)에 입력된다. 알고리즘(1030)은 최상의 요각 추정치(1040) 및 최상의 요각 추정치(1040)와 연관된 오차(1050)를 출력한다.

최상의 요각 추정치를 획득하기 위해 상이한 요각 추정치들을 융합시키는 방법에 관계된 알고리즘에 대한 이하의 설명에서, 인덱스 n은 시간 스텝 n에서의 값을 나타낸다.

방법의 일부 실시예는 요각 영역에서 동작하는 1차원 적응 필터를 사용한다. 선택적으로, 융합시키는 방법이 수행되어야 하는지(즉, 자력계로부터의 요각 추정치를 유지해야 하는지) 여부를 나타내기 위해 부울 변수(예컨대, “noYawCorrectFromMag_”라고 함)가 사용될 수 있다. 부울 변수의 값이 사전 결정된 조건(들)이 충족되는지에 따라 기본값과 다른 값 사이에서 토글될 수 있다. 방법은 다음과 같은 단계들을 포함할 수 있다:

단계 1: 장치가 움직이지 않고 있는지에 따라 융합이 사용되어야 하는지를 (다양한 방법들 중 하나를 사용하여) 판정한다(예컨대, noYawCorrectFromMag_를 거짓으로 설정함).

단계 2: 몸체 센서를 사용하여 예측된 요각

을 획득한다. 예를 들어, 3-D 가속도계 및 3-D 자이로스코프를 몸체 센서로서 사용하여 전체 각위치가 추정될 수 있다.

단계 3: (자북과 중력을 사용하여 지구-고정 중력 기준계를 정의하는 방식에 따라) 교정되고 근방계 보상된 자계 추정치를, 자북과 기준 요-제로(reference yaw-zero) 사이의 상대 초기 요각 오프셋과 함께 사용하여, 요각 추정치

을 계산한다.

단계 4: 다음 중 하나 이상을 고려하여 총 추정치 오차

를 계산한다:

a. 교정 정확도

b. 센서 잡음, 롤 및 피치 추정치 오차로부터 얻어지는 요각 오차 계산

c. 근방계 보상 오차

단계 5: 단계 2 및 단계 3으로부터의 요각 추정치

및

을 적응 필터에의 입력으로서 사용하여 적응 필터의 보정 방식을 적용한다. 적응 필터의 출력은 요각의 최상의 추정치

이다. 적응 필터의 계수 totalK가 다음의 절차들 중 임의의 절차 또는 그 절차들의 임의의 조합의 생성물을 사용하여 계산될 수 있다.

절차 1: K₁은 일반적으로 단계 4에서 계산된 totError

에 대한 혁신

의 비의 함수이다. 혁신은 자력계로부터의 현재의 요각

과 적응 필터의 마지막 상태로부터의 예측된 최상의 요각 추정치

사이의 차이다.

예시적인 실시예에서, K₁은 “totError”

에 대한 혁신

의 비의 3차 다항식 함수이다.

여기서 K₁은 0과 1 사이로 제한된다.

절차 2: K₂는 totError의 제곱

에 대한 몸체 센서(예컨대, 자이로스코프)에서의 예측된 요 분산

의 비이다.

절차 3: “totError”

이 임계값

이하인 경우, K₃은 1이고, 그렇지 않은 경우, 몸체 센서(에컨대, 자이로)의 예측된 요 오차에 대한 혁신의 비의 함수이다. 예를 들어,

K₃ 계산의 예시적인 실시예가 다음과 같이 주어져 있다:

코드 9

절차 4: 혁신의 절대값

이 임계값

보다 큰 경우, K₄가 1이고, 그렇지 않은 경우, 0.001과 같은 작은 값의 상수이다.

단계 6: totalK(

)을 계산한다. 예를 들어,

특정의 조건이 충족되면, totalK는 0으로 설정된다. 이러한 조건은 다음과 같다:

1) 혁신의 절대값

이 교정의 정확도보다 작다;

2) 총 추정치 오차 “totError”

가 임계값

보다 크다;

3) 멤버 변수 noYawCorrectFromMag_가 참이다;

4) IIR 저역 통과 필터링된 버전과 추정된 자계로부터의 측정된 요각의 순간 버전 사이의 차이가 사전 결정된 임계값(예컨대, 0.04 라디안)보다 크다.

최상의 요 추정치가 다음과 같이 계산된다:

또는

여기서,

은

의 함수이다. 한 예시적인 실시예에서, 점 [0, 0.002] 및 [4, 1]을 통과하는 비선형 곡선이 사용되고 1에서 포화된다. 다른 예시적인 실시예에서,

이다. 자력계로부터의 요각 추정치의 오차가 잘 제한되어 있는 경우, 이는 항상 잘 제한된 정확도를 갖는 요각을 제공하고, 따라서 관성 센서(예컨대, 3-D 자이로스코프)로부터 추정된 요각의 임의적인 큰 드리프트를 보정하는 데 도움을 줄 수 있다. 필터가 적응적이기 때문에, 각각의 단계에 대한 보정량이 동적이고, 요 오차를 훨씬 더 빨리 감소시키는 데 도움을 줄 수 있지만 장치가 움직이고 있지 않을 때 여전히 안정적이다.

단계 7: 선택적으로, 응용이 각위치를 사용하는 경우, 보정된 요각을 갖는 오일러각을 다시 4원수(전체 각위치)로 변환한다.

단계 8: 선택적으로, (1) 보정된 요각과 추정된 자계를 사용하여 측정된 요각 사이의 차이가 사전 결정된 임계값(예컨대, 0.02 라디안) 이하임과 동시에 (2) 장치가 움직이지 않는 것(장치가 손에 들려 있고 떨림만이 검출될 때 참으로 간주될 수 있음)으로 검출되는 경우에, noYawCorrectFromMag_가 참으로 설정된다.

전술한 방법들은 개별적으로 또는 결합되어 구현될 수 있다. 예시적인 실시예에 따른, 장치에 부착된 움직임 센서 및 자력계를 사용하여 중력 기준계에 대한 장치의 몸체 기준계의 요각을 추정하는 방법(1100)의 흐름도가 도 13에 예시되어 있다. "움직임 센서"라는 용어는 롤 및 피치의 측정, 및 적어도 상대 요(즉, 요의 원시 추정치)를 제공할 수 있는 임의의 감지 요소(들)를 의미한다.

이 방법(1100)은 움직임 센서로부터 그리고 자력계로부터 측정치를 수신하는 단계(S1110)를 포함한다. 수신된 측정치는 동시적인 측정치일 수 있다. "동시적인"이라는 용어는 동일한 시간 구간 또는 시간 스텝에서를 의미한다.

이 방법(1100)은 수신된 측정치에 기초하여, 측정된 3-D 자계, 롤각, 피치각, 및 몸체 기준계에서의 장치의 요각의 원시 추정치를 구하는 단계(S1120)를 추가로 포함한다. 여기서, "측정된 3-D 자계"라는 용어는 자력계로부터 수신된 측정치(신호)에 기초하여 결정된 벡터값을 의미한다. 상수이거나 자력계의 교정 절차 동안 결정되는 다양한 파라미터가 측정된 3-D 자계를 구하는 데 사용될 수 있다. 이와 유사하게, 현재의 롤, 피치 및 요의 원시 추정치가 움직임 센서로부터 수신된 측정치로부터 그리고 상수이거나 움직임 센서의 교정 절차 동안 결정되는 파라미터를 사용하여 구해진다.

이 방법(1100)은 측정된 3-D 자계로부터 국소 3-D 자계를 추출하는 단계(S1130)를 추가로 포함한다. 연철 효과, 강철 효과 및 몸체 기준계에 대한 자력계의 상대 정렬 중 하나 이상에 대해 국소 3-D 자계가 보정될 수 있다. 동적 근방계에 대해 국소 3-D 자계가 보상된다.

이 방법(1100)은 이어서 적어도 2개의 상이한 방법을 사용하여, 추출된 국소 3-D 자계, 롤각, 피치각 및 요각의 원시 추정치에 기초하여 중력 기준계에서의 장치의 몸체 기준계의 틸트-보상된 요각을 계산하는 단계 - 롤각 추정치의 오차, 피치각 추정치의 오차, 및 추출된 국소 3-D 자계의 오차는 적어도 2개의 상이한 방법에 대해 상이하게 틸트-보상된 요각의 오차에 영향을 미침 - (S1140)를 포함한다. 이 동작이, 전술한 예시적인 실시예에 따른, 롤 및 피치를 사용하여 보상된 틸트를 갖는 요각을 계산하는 방법들 또는 최상의 요각 추정치를 얻기 위해 상이한 요각 추정치들을 융합시키는 방법들 중 임의의 방법을 사용하여 수행될 수 있다.

예시적인 실시예에 따른, 장치에 부착된 움직임 센서 및 자력계의 동시적인 측정치를 사용하여 자력계를 교정하는 방법(1200)의 흐름도가 도 14에 예시되어 있다. 이 방법(1200)은 움직임 센서로부터 그리고 자력계로부터 일련의 동시적인 측정치를 수신하는 단계(S1210)를 포함한다.

이 방법(1200)은 자력계로부터 수신된 일련의 동시적인 측정치 중의 측정치에 기초하여 측정된 자계를 계산하는 파라미터를 결정하는 단계 - 이 결정하는 단계는 움직임 센서로부터 수신된 일련의 동시적인 측정치 중의 측정치로부터 획득된 현재의 롤, 피치 및 상태 요를 사용하여 수행되고, 파라미터들 중 적어도 일부가 분석적으로 결정됨 - (S1220)를 추가로 포함한다. 이 동작은, 전술한 예시적인 실시예에 따른, 자세-독립적 파라미터를 결정(교정)하는 방법들 및 자세-의존적 파라미터를 결정(교정)하는(즉, 자력계를 정렬하는) 방법들 중 임의의 방법을 사용하여 수행될 수 있다.

개시된 예시적인 실시예는 장치의 배향을 결정하기 위해 자력계가 다른 센서와 함께 사용될 때 사용가능한 툴킷의 일부일 수 있는 방법, 및 이 툴킷을 사용할 수 있는 시스템을 제공한다. 이 방법은 컴퓨터 프로그램 제품에 구현되어 있을 수 있다. 이 설명이 본 발명을 제한하기 위한 것이 아님을 잘 알 것이다. 이와 반대로, 예시적인 실시예가 첨부된 특허청구항범위에 의해 한정되는 본 발명의 사상 및 범위 내에 포함될 수 있는 대안, 수정 및 등가물을 포함하는 것으로 보아야 한다. 게다가, 예시적인 실시예에 대한 상세한 설명에서, 청구된 발명의 포괄적인 이해를 제공하기 위해 다수의 구체적인 상세가 기술되어 있다. 그렇지만, 당업자라면 다양한 실시예가 이러한 구체적인 상세 없이 실시될 수 있다는 것을 잘 알 것이다.

예시적인 실시예는 전적으로 하드웨어인 실시예 또는 하드웨어 측면과 소프트웨어 측면을 겸비하는 실시예의 형태를 취할 수 있다. 게다가, 예시적인 실시예는 컴퓨터 판독가능 저장 매체에 저장되는 컴퓨터 프로그램 제품의 형태 - 컴퓨터 판독가능 명령어가 매체에 구현되어 있음 - 를 취할 수 있다. 하드 디스크, CD-ROM, DVD(digital versatile disc), 광 저장 장치, 또는 자기 저장 장치(플로피 디스크 또는 자기 테이프 등)를 비롯한 임의의 적당한 컴퓨터 판독가능 매체가 이용될 수 있다. 컴퓨터 판독가능 매체의 다른 비제한적인 일례는 플래시-타입 메모리 또는 기타 공지된 메모리를 포함한다.

이 예시적인 실시예의 특징 및 요소가 실시예에서 특정의 조합으로 기술되어 있지만, 각각의 특징 또는 요소가 실시예의 다른 특징 및 요소 없이 단독으로 또는 본 명세서에 개시된 다른 특징 및 요소를 갖거나 갖지 않는 다양한 조합으로 사용될 수 있다. 본 출원에서 제공된 방법 또는 플로우차트는 구체적으로 프로그램된 컴퓨터 또는 프로세서에 의해 실행하기 위해 컴퓨터 판독가능 저장 매체에 유형적으로 구현되는 컴퓨터 프로그램, 소프트웨어, 또는 펌웨어로 구현될 수 있다.

Claims

장치에 부착된 움직임 센서 및 자력계를 사용하여 중력 기준계에 대한 상기 장치의 몸체 기준계의 요각을 추정하는 방법(1100)으로서,
상기 움직임 센서로부터 그리고 상기 자력계로부터 측정치를 수신하는 단계(S1110);
상기 수신된 측정치에 기초하여, 측정된 3-D 자계, 롤각, 피치각, 및 상기 몸체 기준계에서의 상기 장치의 요각의 원시 추정치를 구하는 단계(S1120);
상기 측정된 3-D 자계로부터 국소 3-D 자계를 추출하는 단계(S1130); 및
적어도 2개의 상이한 방법을 사용하여, 상기 추출된 국소 3-D 자계, 상기 롤각, 상기 피치각 및 상기 요각의 원시 추정치에 기초하여 상기 중력 기준계에서의 상기 장치의 상기 몸체 기준계의 틸트-보상된 요각을 계산하는 단계(S1140)를 포함하고, 상기 롤각 추정치의 오차, 상기 피치각 추정치의 오차, 및 상기 추출된 국소 3-D 자계의 오차가 상기 적어도 2개의 상이한 방법에 대해 상이하게 상기 틸트-보상된 요각의 오차에 영향을 미치는 것인 방법.
제1항에 있어서, 연철 효과, 강철 효과 및 상기 몸체 기준계에 대한 상기 자력계의 상대 정렬 중 하나 이상에 대해 상기 국소 3-D 자계가 보정되는 것인 방법.
제1항에 있어서, 동적 근방계에 대해 상기 국소 3-D 자계가 보상되는 것인 방법.
제1항에 있어서, 상기 중력 기준계가 중력 및 지구의 자계 방향에 대해 정의되는 지구-고정 직교 기준계인 방법.
제1항에 있어서, 상기 수신된 측정치가 동시적인 측정치인 방법.
제3항에 있어서, 상기 측정된 3-D 자계의 진화(evolution)를 추적한 것에 기초하여 동적 근방계에 대해 상기 국소 3-D 자계가 보상되는 것인 방법.
제1항에 있어서, 상기 측정된 3-D 자계가 센서의 고유 특성에 관계된 파라미터를 사용하여 계산되는 것인 방법.
제7항에 있어서, 센서의 고유 특성에 관계된 상기 파라미터가 오프셋, 스케일, 및 스큐/교차 결합 행렬 중 하나 이상을 포함하는 것인 방법.
제1항에 있어서, 상기 움직임 센서가 가속도계를 포함하고 상기 가속도계의 측정치가 중력에 대한 상기 장치의 상기 몸체 기준계의 틸트를 결정하는 데 사용되는 것인 방법.
제1항에 있어서, 상기 계산하는 단계가 상기 틸트-보상된 요각의 오차를 추정하는 단계를 포함하는 것인 방법.
제1항에 있어서, 상기 계산하는 단계가
상기 장치에 관계되어 있고 중력을 따라 z-축을 가지는 다른 기준계에서 롤 및 피치를 획득하는 단계, 및
상기 중력 기준계에서의 정적 자계를 추정하는 단계를 포함하는 것인 방법.
제11항에 있어서, 상기 획득하는 단계가 상기 정적 국소 자계와 중력의 반대 방향 사이의 각도를 추정하는 단계를 포함하는 것인 방법.
제1항에 있어서, 상기 적어도 2개의 방법 각각을 사용하여 계산된 상기 틸트-보상된 요각의 오차가 추정되고, 상기 추정된 오차들 중 가장 작은 것에 대응하는 상기 틸트-보상된 요각의 값이 출력되는 것인 방법.
제1항에 있어서, 상기 적어도 2개의 방법 중 하나가 다음과 같이 상기 요각을 계산하고,

여기서
및
은 틸트 보정된 롤 및 피치이며,

여기서
및
는 상기 요의 원시 추정치를 사용하여 계산된 상기 중력 기준계에서의
의 성분이고,

는 상기 추출된 국소 3-D 자계와 중력의 반대 방향 사이의 각도이며,

는 상기 몸체 기준계에서의 상기 국소 3-D 자계의 추정치이고,

는 상기 몸체 기준계에서의 상기 국소 3-D 자계의 중력에 평행한 성분의 추정치이며,

는 상기 몸체 기준계에서의 상기 국소 3-D 자계의 중력에 수직인 성분의 추정치인 방법.
제1항에 있어서, 상기 적어도 2개의 방법 중 하나가 다음과 같이 상기 요각을 계산하고,

여기서
및
은 틸트 보정된 롤 및 피치이며,

, 여기서
및
는 상기 요의 원시 추정치를 사용하여 계산된 상기 중력 기준계에서의
의 성분이고,

는 상기 추출된 국소 3-D 자계와 중력의 반대 방향 사이의 각도이며,

은 상기 몸체 기준계에서의 상기 국소 3-D 자계의 추정치이고,

은 상기 몸체 기준계에서의 상기 국소 3-D 자계의 중력에 평행한 성분의 추정치이며,

은 상기 몸체 기준계에서의 상기 국소 3-D 자계의 중력에 수직인 성분의 추정치인 방법.
제1항에 있어서, 상기 적어도 2개의 방법 중 하나가 다음과 같이 상기 요각을 계산하고,

여기서
및
은 틸트 보정된 롤 및 피치이며,

, 여기서
및
는 상기 요의 원시 추정치를 사용하여 계산된 상기 중력 기준계에서의
의 성분이고,

는 상기 추출된 국소 3-D 자계와 중력의 반대 방향 사이의 각도이며,

은 상기 몸체 기준계에서의 상기 국소 3-D 자계의 추정치이고,

은 상기 몸체 기준계에서의 상기 국소 3-D 자계의 중력에 평행한 성분의 추정치이며,

은 상기 몸체 기준계에서의 상기 국소 3-D 자계의 중력에 수직인 성분의 추정치인 방법.
제6항에 있어서, 상이한 시간 스텝에 대응하는 상기 측정된 3-D 자계의 제1 값 및 자계 모델을 사용하여 예측되는 상기 자계의 제2 값을 사용하여 동적 근방계가 추적되고, 상기 측정된 3-D 자계가 상기 자계 모델이 예측하는 것과 상이한지를 판정하기 위해 상기 제1 값 및 상기 제2 값이 비교되는 것인 방법.
제17항에 있어서, 비교의 결과가 상기 측정된 3-D 자계가 상기 자계 모델이 예측하는 것과 상이하지 않다는 것인 경우, 요각의 오차가 추정되는 것인 방법.
제17항에 있어서, 비교의 결과가 상기 측정된 3-D 자계가 상기 자계 모델이 예측하는 것과 상이하지 않다는 것인 경우, 롤각의 오차가 추정되는 것인 방법.
제17항에 있어서, 비교의 결과가 상기 측정된 3-D 자계가 상기 자계 모델이 예측하는 것과 상이하지 않다는 것인 경우, 피치각의 오차가 추정되는 것인 방법.
제17항에 있어서, 비교의 결과가 상기 측정된 3-D 자계가 상기 자계 모델이 예측하는 것과 상이하다는 것인 경우, 상기 자계 모델이 갱신되는 것인 방법.
제1항에 있어서,
상기 움직임 센서가 관성 센서를 포함하고 - 상기 관성 센서의 측정치는 관성 센서 요각을 산출함 -,
상기 계산하는 단계가 상기 틸트-보상된 요각 및 상기 관성 센서 요각에 기초하여 최상의 요각 추정치를 결정하는 단계를 포함하며,
상기 최상의 요각 추정치를 결정하는 단계가 상기 틸트-보상된 요각 및 상기 관성 센서 요각과 연관된 오차를 계산하는 단계를 포함하는 것인 방법.
제22항에 있어서, 상기 결정하는 단계가 적응 필터를 사용하여 상기 틸트-보상된 요각과 상기 관성 센서 요각을 결합시키는 단계를 포함하는 것인 방법.
제23항에 있어서, 상기 결정하는 단계가 교정 정확도, 센서 잡음으로 인한 요각 계산 오차, 롤 및 피치 추정치 오차, 및 근방계 보상 오차 중 하나 이상에 기초하여 계산된 총 추정치 오차를 사용하여 적응 필터의 이득 계수를 계산하는 단계를 포함하는 것인 방법.
제24항에 있어서, 상기 적응 필터의 계수가 혁신 변수(innovation variable)의 절대값을 상기 총 추정치 오차로 나눈 비이고, 상기 혁신 변수는 자력계 측정치로부터 추론되는 현재의 요각과 상기 적응 필터의 이전의 출력으로부터 예측된 최상의 요각 추정치 사이의 차이인 방법.
제24항에 있어서, 상기 적응 필터의 계수가 상기 관성 센서를 사용할 때의 예측된 요 오차의 제1 제곱과 상기 총 추정치 오차의 제2 제곱의 비인 방법.
제24항에 있어서, 상기 적응 필터의 계수는 상기 총 추정치 오차가 사전 결정된 임계값보다 작은 경우 1이고, 그렇지 않은 경우, 혁신 변수의 절대값을 상기 관성 센서를 사용할 때의 예측된 요각 오차로 나눈 비의 함수이며, 상기 혁신 변수는 자력계 측정치로부터 추론되는 현재의 요각과 상기 적응 필터의 이전의 출력으로부터 예측된 최상의 요각 추정치 사이의 차이인 방법.
제24항에 있어서, 상기 적응 필터의 계수는 혁신 변수가 사전 결정된 임계값보다 작은 경우 1이고, 그렇지 않은 경우, 사전 결정된 작은 값인 방법.
제24항에 있어서, 상기 적응 필터의 계수가
(1) 혁신 변수의 절대값을 상기 총 추정치 오차로 나눈 비,
(2) 상기 관성 센서를 사용할 때의 예측된 요 오차의 제1 제곱과 상기 총 추정치 오차의 제2 제곱의 비,
(3) 상기 총 추정치 오차가 제1 사전 결정된 임계값보다 작은 경우 1, 및 그렇지 않은 경우, 상기 혁신 변수의 절대값을 상기 관성 센서를 사용할 때의 상기 예측된 요각 오차로 나눈 비의 함수, 및
(4) 상기 혁신 변수가 제2 사전 결정된 임계값보다 작은 경우 1, 및 그렇지 않은 경우, 사전 결정된 작은 값 중 2개 이상의 곱이고,
상기 혁신 변수는 자력계 측정치로부터 추론되는 현재의 요각과 상기 적응 필터의 이전의 출력으로부터 예측된 최상의 요각 추정치 사이의 차이인 방법.
제24항에 있어서, 상기 최상의 요각 추정치가 (A) 이전의 단계로부터의 최상의 요 추정치에 기초하여 상기 관성 센서로부터의 예측된 요각과 (B) 혁신 변수와 상기 적응 필터의 계수의 함수의 곱의 합이고, 상기 혁신 변수는 자력계 측정치로부터 추론되는 현재의 요각과 상기 적응 필터의 이전의 출력으로부터 예측된 최상의 요각 추정치 사이의 차이인 방법.
장치(100)로서,
강체(101)를 가지는 디바이스;
상기 디바이스 상에 탑재되어 있고 국소 자계에 대응하는 측정치를 발생하도록 구성되어 있는 3-D 자력계(120);
상기 디바이스 상에 탑재되어 있고 상기 강체의 배향에 대응하는 측정치를 발생하도록 구성되어 있는 움직임 센서(110); 및
적어도 하나의 처리 유닛(130)을 포함하며, 상기 적어도 하나의 처리 유닛은
(1) 상기 움직임 센서로부터 그리고 상기 자력계로부터 측정치를 수신하고,
(2) 상기 수신된 측정치에 기초하여, 측정된 3-D 자계, 롤각, 피치각, 및 몸체 기준계에서의 상기 디바이스의 요각의 원시 추정치를 구하고,
(3) 상기 측정된 3-D 자계로부터 국소 3-D 자계를 추출하고,
(4) 적어도 2개의 상이한 방법을 사용하여, 상기 추출된 국소 3-D 자계, 상기 롤각, 상기 피치각 및 상기 요각의 원시 추정치에 기초하여 중력 기준계에서의 상기 디바이스의 상기 몸체 기준계의 틸트-보상된 요각을 계산하도록 구성되어 있고, 상기 롤각 추정치의 오차, 상기 피치각 추정치의 오차, 및 상기 추출된 국소 3-D 자계의 오차가 상기 적어도 2개의 상이한 방법에 대해 상이하게 상기 틸트-보상된 요각의 오차에 영향을 미치는 것인 장치.
제31항에 있어서, 상기 적어도 하나의 처리 유닛이 (1) 내지 (4) 중 적어도 하나를 실행하도록 구성되어 있는 상기 디바이스 내에 배치된 처리 유닛을 포함하는 것인 장치.
제31항에 있어서, 상기 적어도 하나의 처리 유닛이 (1) 내지 (4) 중 적어도 하나를 실행하도록 구성되어 있는, 원격지에 위치한 처리 유닛을 포함하고, 상기 장치는 원격지에 위치해 있는 상기 처리 유닛으로 데이터를 전송하도록 구성되어 있는, 상기 디바이스 상에 탑재된 송신기를 추가로 포함하는 것인 장치.
컴퓨터 상에서 실행될 때 상기 컴퓨터로 하여금 장치에 부착된 움직임 센서 및 자력계를 사용하여 중력 기준계에 대한 상기 장치의 몸체 기준계의 요각을 추정하는 방법을 수행하게 하는 실행가능 코드를 저장하도록 구성되어 있는 컴퓨터 판독가능 저장 매체(135)로서, 상기 방법이
상기 움직임 센서로부터 그리고 상기 자력계로부터 측정치를 수신하는 단계(S1110);
상기 수신된 측정치에 기초하여, 측정된 3-D 자계, 롤각, 피치각, 및 상기 몸체 기준계에서의 상기 장치의 요각의 원시 추정치를 구하는 단계(S1120);
상기 측정된 3-D 자계로부터 국소 3-D 자계를 추출하는 단계(S1130); 및
적어도 2개의 상이한 방법을 사용하여, 상기 추출된 국소 3-D 자계, 상기 롤각, 상기 피치각 및 상기 요각의 원시 추정치에 기초하여 상기 중력 기준계에서의 상기 장치의 상기 몸체 기준계의 틸트-보상된 요각을 계산하는 단계(S1140)를 포함하고, 상기 롤각 추정치의 오차, 상기 피치각 추정치의 오차, 및 상기 추출된 국소 3-D 자계의 오차가 상기 적어도 2개의 상이한 방법에 대해 상이하게 상기 틸트-보상된 요각의 오차에 영향을 미치는 것인 컴퓨터 판독가능 저장 매체.