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

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

Info

Publication number
KR20130143576A
KR20130143576A KR1020137011278A KR20137011278A KR20130143576A KR 20130143576 A KR20130143576 A KR 20130143576A KR 1020137011278 A KR1020137011278 A KR 1020137011278A KR 20137011278 A KR20137011278 A KR 20137011278A KR 20130143576 A KR20130143576 A KR 20130143576A
Authority
KR
South Korea
Prior art keywords
magnetic field
reference system
estimate
angle
error
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Withdrawn
Application number
KR1020137011278A
Other languages
English (en)
Inventor
후아 셍
Original Assignee
힐크레스트 래보래토리스, 인크.
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 힐크레스트 래보래토리스, 인크. filed Critical 힐크레스트 래보래토리스, 인크.
Publication of KR20130143576A publication Critical patent/KR20130143576A/ko
Withdrawn legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64D—EQUIPMENT FOR FITTING IN OR TO AIRCRAFT; FLIGHT SUITS; PARACHUTES; ARRANGEMENT OR MOUNTING OF POWER PLANTS OR PROPULSION TRANSMISSIONS IN AIRCRAFT
    • B64D45/00—Aircraft indicators or protectors not otherwise provided for
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B7/00—Measuring arrangements characterised by the use of electric or magnetic techniques
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64D—EQUIPMENT FOR FITTING IN OR TO AIRCRAFT; FLIGHT SUITS; PARACHUTES; ARRANGEMENT OR MOUNTING OF POWER PLANTS OR PROPULSION TRANSMISSIONS IN AIRCRAFT
    • B64D47/00—Equipment not otherwise provided for
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B7/00—Measuring arrangements characterised by the use of electric or magnetic techniques
    • G01B7/003—Measuring arrangements characterised by the use of electric or magnetic techniques for measuring position, not involving coordinate determination
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B7/00—Measuring arrangements characterised by the use of electric or magnetic techniques
    • G01B7/30—Measuring arrangements characterised by the use of electric or magnetic techniques for measuring angles or tapers; for testing the alignment of axes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C17/00—Compasses; Devices for ascertaining true or magnetic north for navigation or surveying purposes
    • G01C17/38—Testing, calibrating, or compensating of compasses
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1654—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with electromagnetic compass
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/40—Control within particular dimensions
    • G05D1/49—Control of attitude, i.e. control of roll, pitch or yaw
    • G05D1/495—Control of attitude, i.e. control of roll, pitch or yaw to ensure stability

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Measuring Magnetic Variables (AREA)
  • Measurement Of Length, Angles, Or The Like Using Electric Or Magnetic Means (AREA)

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)에 관계된 알고리즘을 설명하는 데 사용되는 표기법의 목록이다.
Figure pct00001
Figure pct00002
Figure pct00003
도 5를 고려하면, 지구-고정 중력 기준계를 현재의 장치의 몸체 기준계로 만드는 회전 행렬
Figure pct00004
은 3번의 회전을 포함하는 오일러각 시퀀스(Euler angle sequence)이고, 하기 수학식 1로 주어진다.
Figure pct00005
도 5에 예시된 바와 같이, 지구-고정 중력 기준계에서의 자계는 수학식 2로 표현될 수 있고,
Figure pct00006
여기서,
Figure pct00007
는 벡터
Figure pct00008
와 벡터
Figure pct00009
사이의 각도이고, 이는 수학식 3에 의해 복각
Figure pct00010
에 관계되어 있다.
Figure pct00011
3-D 교정된 자력계 측정치(310)는 수학식 4로 표현될 수 있고,
Figure pct00012
여기서,
Figure pct00013
는 수학식 5이며,
Figure pct00014
Figure pct00015
은
Figure pct00016
의 결합 확률 밀도 함수를 갖는 백색 가우시안 측정 잡음이다.
수학식 1 및 수학식 2를 수학식 5에 대입하면, 실제 자계(잡음이 없음)는 수학식 6이다.
Figure pct00017
이어서, 정규화된
Figure pct00018
은 수학식 7에 의해 주어진다.
Figure pct00019
정규화된
Figure pct00020
은 중력에 평행한 성분
Figure pct00021
과 중력에 수직인 성분
Figure pct00022
의 합이다.
유의할 점은 (1) 중력에 평행한 성분
Figure pct00023
가 요각
Figure pct00024
에 관한 정보를 전달하지 않고, (2) 각도
Figure pct00025
가
Figure pct00026
와 평행한 정규화된 성분의 마이너스
Figure pct00027
사이의 각도이다.
Figure pct00028
따라서, 보정된 입력 틸트각
Figure pct00029
및
Figure pct00030
이 주어지면,
Figure pct00031
은 이어서
Figure pct00032
을 계산하기 위해 보정된 자력계 입력
Figure pct00033
과 함께 사용된다.
Figure pct00034
추정된
Figure pct00035
을 사용하고 수학식 10 및 수학식 11을 수학식 7에 대입하면, 하기의 관계식이 얻어진다.
Figure pct00036
수학식 12에 기초하여, 요각을 계산하기 위해 종래의 방법과 상이한 3개의 방법이 여기에서 제안된다. 하기의 수학식들을 단순화시키기 위해, 수학식 13을 정의하기로 한다.
Figure pct00037
Figure pct00038
와
Figure pct00039
의 Z 성분의 곱으로부터
Figure pct00040
과
Figure pct00041
의 Y 성분의 곱을 감산함으로써, 수학식 14가 얻어진다.
Figure pct00042
이와 유사하게,
Figure pct00043
과
Figure pct00044
의 Z 성분의 곱을
Figure pct00045
과
Figure pct00046
의 Y 성분의 곱에 가산함으로써, 수학식 15가 얻어진다.
Figure pct00047
Figure pct00048
의 X 성분은 수학식 16이다.
Figure pct00049
요각
Figure pct00050
을 계산하는 제1 방법에서, 수학식 14가
Figure pct00051
과 곱해지고 수학식 15에 의해 나누어져 수학식 17을 얻는다.
Figure pct00052
요각
Figure pct00053
을 계산하는 제2 방법에서, 수학식 14가
Figure pct00054
과 곱해지고 수학식 16에 의해 나누어져 수학식 18을 얻는다.
Figure pct00055
요각
Figure pct00056
을 계산하는 제3 방법에서, 수학식 14 내지 수학식 16이 결합되어 수학식 19를 산출한다.
Figure pct00057
일 실시예에서, 이 알고리즘은 상기 3가지 방법 중 최종적인
Figure pct00058
에 대해 최고의 정확도를 갖는 방법을 동적으로 선택하는데, 그 이유는 3가지 방법에 대한 오차가 각각의 채널에 따른 자력계 잡음과 입력 롤각 및 피치각의 오차 둘 다의 상이한 함수이기 때문이고(어떤 방법은 어떤 오차 소스에 의해 더 영향을 받는 반면 다른 오차 소스에 의해 덜 영향을 받음), 예컨대, 방법 1은 자력계의 x-축 측정치의 오차에 영향을 받지 않고, 방법 2는
Figure pct00059
의 오차에 대한 함수이고, 따라서, 피치각이 0도에 가까울 때, 방법 2는 피치의 오차에 덜 민감하다). 일 실시예에서, 이 방법은 다음과 같이 동적으로 선택될 수 있다: (1) 피치각의 절대값이 0 내지 π/4인 경우, 제2 방법을 사용함; (2) 피치각의 절대값이 π/3 내지 π/2인 경우, 제1 방법을 사용함; (3) 그렇지 않은 경우, 제3 방법을 사용함. 이 방식에 의해 각각의 개별 영역에서 장치의 배향에 덜 민감한 보다 안정된 요각이 얻어진다. 유의할 점은, 이 동일한 기본 방식이 방정식들에서의 각각의 요소의 예상된 정확도에 기초하여 다양한 추정치를 병합하는 단일 방정식으로 구현될 수 있다는 것이다. 또한, 유의할 점은 이 동일한 방식이 자력계 측정치를 사용한 피치 및 롤의 계산에서 사용될 수 있다는 것이다.
참조를 위해, 종래의 방법은 하기의 수식을 사용하여
Figure pct00060
을 계산한다.
Figure pct00061
이 종래의 계산은 모든 오차 소스(즉, 롤각의 오차, 피치각의 오차, 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는 자세-독립적 파라미터를 교정하는 방법에 관계된 알고리즘을 설명하는 데 사용되는 표기법의 목록이다.
Figure pct00062
Figure pct00063
자력계의 감지 요소에 의해 검출되는 신호는 그 근방에 강자성 원소가 존재하는 것에 의해 왜곡된다. 예를 들어, 자계와 둘러싸고 있는 설치 재료 사이의 간섭에 의해, 국소 영구 자화된 재료에 의해, 센서 자체의 스케일링, 교차 결합, 바이어스에 의해, 그리고 센서의 기술적 한계 등에 의해 신호가 왜곡된다. 자기 왜곡 및 감지 오차의 유형 및 효과는 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로서 모델링되었다.
Figure pct00064
참조 문헌 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이고,
Figure pct00065
여기서 D는 센서 기여분 및 연철 효과 둘다로부터의 스케일링 및 스큐를 결합시킨 것이고, O는 지구-고정 중력 기준계에 대한 연철 효과 및 센서의 내부 정렬 오차 둘 다를 결합시킨 오정렬 행렬이며, b는 강철 효과 및 센서의 고유 기여분 둘 다로 인한 바이어스이고, n은 영 평균 및 일정한 표준 편차 σ를 갖는 변환된 센서 측정 잡음 벡터이다.
O 및
Figure pct00066
둘 다가 벡터의 방향만을 변경시키기 때문에,
Figure pct00067
의 크기는 지구-고정 몸체 기준계에 대한 자력계의 배향의 상수 불변량이다. 점
Figure pct00068
가 구(sphere)로 제약되는 경우, 자력계 판독치 Bk는 타원체 상에 있다.
임의의 Bk 집합(즉, 타원체의 임의의 부분)에 대해, 수학적 닫힌 형태에 의해 D와 b를 동시에 분석적으로 구하는 방법이 제공된다. 수학식 22는 수학식 23으로 바꾸어 쓸 수 있다.
Figure pct00069
수학식 23의 양변에서의 크기 제곱도 또한 같고, 수학식 24를 산출한다.
Figure pct00070
Figure pct00071
이기 때문에, 수학식 24가 수학식 25로 바꾸어 쓸 수 있다.
Figure pct00072
수학식 25의 우변이 잡음 항인 경우, 수학식 25에 대한 해는 수학식 26과 같이
Figure pct00073
내지
Figure pct00074
의 최소 제곱 근사(least square fit)일 수 있다.
Figure pct00075
그렇지만, 수학식 26이 D와 b의 고비선형 함수(highly nonlinear function)이기 때문에, 간단한 선형 분석해가 없다.
수학식 25에서의 잡음을 무시한 정의 - 수학식 27 및 수학식 28 - 를 사용하고,
Figure pct00076
Figure pct00077
수학식 29를 전개함으로써,
Figure pct00078
하기의 관계식이 얻어진다.
Figure pct00079
수학식 30을 간략화하기 위해, Q 원소가 수학식 31과 같이 정의된다.
Figure pct00080
그러면, 수학식 28에 기초하여, E는 수학식 32가 된다.
Figure pct00081
행렬 pD는 SVD(singular value decomposition, 특이값 분해)법을 사용하여 구해질 수 있고,
Figure pct00082
여기서, s는 3x3 대각 행렬이다. 이어서 S의 각각의 원소에 제곱근을 적용하면, 다음과 같이 또 하나의 3x3 대각 행렬 w가 얻어지고, 이어서 pD가 얻어진다.
Figure pct00083
Figure pct00084
오프셋 b는 수학식 36과 같이 계산된다.
Figure pct00085
Q를 구하기 위해, 3개의 크기 Q(1), Q(2), 및 Q(3)에 걸친 평균이 수학식 37과 같이 정의된다.
Figure pct00086
새로운 파라미터 벡터 K를 사용하면,
Figure pct00087
수학식 29는 수학식 39로 된다.
Figure pct00088
다음과 같이 Nx9 행렬 T 및 Nx1 벡터 U를 정의하자.
Figure pct00089
Figure pct00090
이 표기법을 사용하면, N개의 샘플 측정치에 대해, 수학식 39는 수학식 42로 되고,
Figure pct00091
수학식 43에 의해 해가 구해질 수 있다.
Figure pct00092
이어서, 수학식 38 및 수학식 32로부터, E는 수학식 44와 같이 쓰여질 수 있다.
Figure pct00093
수학식 45를 정의하자.
Figure pct00094
G는 수학식 33 내지 수학식 35를 사용하여 pD와 동일한 방식으로 구해지고,
Figure pct00095
b는 수학식 36, 수학식 38 및 수학식 46을 결합함으로써 계산된다.
Figure pct00096
수학식 38에서의 K(9)의 정의 및 수학식 47을 수학식 31에 대입하면, co는 다음과 같이 계산된다.
Figure pct00097
마지막으로, 수학식 48을 수학식 46 및 수학식 47에, 이어서 수학식 27에 대입하면, D 및 b가 완전히 구해진다.
Figure pct00098
는 국소 지자계 강도의 제곱을 가리킬 수 있다. 심지어 강도가 미지의 값을 가지며, 임의의 임의적인 상수로 사전 설정될 수 있고, 해에 대한 유일한 차이는 3개의 축 모두의 계산된 9개 요소(3 스케일, 3 스큐, 및 3 오프셋) 모두에 대한 상수 스케일 차이이다.
앞서 설명한 형식주의에 기초하여, 예시적인 실시간 구현예에서, 각각의 시간 스텝에 대해, 데이터 수집 엔진(520)은 2개의 변수 행렬을 저장하고: covPInvAccum_이라고 하는 하나의 9x9 행렬은
Figure pct00099
를 누적하는 데 사용되고, zAccum_이라고 하는 다른 변수 9x1 행렬은
Figure pct00100
를 누적하는 데 사용된다. 시간 스텝 n+1에서, 다음과 같은 수학식들에 따라 행렬이 갱신된다.
Figure pct00101
Figure pct00102
T의 n+1 행에 있는 원소인
Figure pct00103
, 및 U의 n+1 행에 있는 원소인
Figure pct00104
은 현재의 시간 스텝에서의 자력계 샘플 측정치만의 함수이다. 이어서, 수학식 43에 기초하여, K가 구해지고, 이어서 수학식 33 내지 수학식 35를 사용하여 G가 구해진다. 임시 변수
Figure pct00105
가 수학식 51과 같이 계산된다.
Figure pct00106
수학식 45의 대입과 함께 이
Figure pct00107
를 수학식 48에 집어넣음으로써, co가 얻어진다.
그에 부가하여, 수학식 51이 수학식 47에 대입되고, 계산된 co가 수학식 46 및 수학식 47에 적용되며, 이어서 수학식 27을 사용하여, D 및 b(즉, 전체 교정 파라미터 집합)가 얻어진다.
이하의 알고리즘이 D 및 b를 구하는 정확도를 결정하는 데 적용될 수 있다. K의 추정치의 오차 공분산 행렬이 수학식 52에 의해 주어지고,
Figure pct00108
여기서,
Figure pct00109
이다.
구해진 파라미터에 대한 K의 야코비 행렬
Figure pct00110
은 다음과 같이 주어진다.
Figure pct00111
Figure pct00112
Figure pct00113
이와 같이, J의 추정치의 오차 공분산 행렬이 수학식 58에 의해 주어진다.
Figure pct00114
추정치 J의 오차는 수학식 59이다.
Figure pct00115
앞서 상세히 기술한 형식주의에 따른 자세-독립적 파라미터를 교정하는 방법이 지구-고정 기준계에서의 상수 물리량 벡터를 측정하는 임의의 센서(지구 중력을 측정하는 가속도계 등)를 교정하는 데 적용될 수 있다. 이들 방법은 임의의 타원체를 구로 근사화시키기 위해 전체 파라미터 집합을 계산하는 데 적용될 수 있고, 여기서 타원체는 원점으로부터 오프셋될 수 있고 및/또는 스큐되어 있을 수 있다. 각각의 샘플 측정에 대해
Figure pct00116
가 알려져 있는 한, 이들 방법이 동적 시변
Figure pct00117
에도 사용될 수 있다.
co를 정의하는 방식이 수학식 37과 상이할 수 있다 - 예를 들어, Q(1), Q(2) 및 Q(3)의 다른 선형 결합이 유사하거나 심지어 더 나은 결과를 가져온다 -. 이러한 선형 결합의 일반 형태는 수학식 60이고:
Figure pct00118
여기서 그 계수들의 합은 1이다. 즉,
Figure pct00119
이다.
수학식 40 및 수학식 41은 상이한 샘플에서의 측정 잡음을 고려하는 것으로 확장될 수 있고, 확장된 수학식은 잡음 분산의 역을 가중치로서 사용한다.
Figure pct00120
Figure pct00121
측정 오차의 다른 기능은 또한 유사한 방식으로 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은 자세-의존적 파라미터를 교정하는 방법에 관계된 알고리즘을 설명하는 데 사용되는 표기법의 목록이다.
Figure pct00122
Figure pct00123
Figure pct00124
Figure pct00125
정렬 오차의 주요 원인은 장치에 대한 자력계의 불완전 설치(즉, 장치의 몸체 기준계에 대한 오정렬), 및 연철 효과로부터의 영향이다. 시간 스텝 tn에서의 자세 독립적 교정된 자력계 측정치는 수학식 64이고,
Figure pct00126
여기서
Figure pct00127
은 수학식 65로 분해될 수 있다.
Figure pct00128
Figure pct00129
은 자력계의 측정치와 장치의 몸체 기준계 사이의 오정렬 행렬이고,
Figure pct00130
은 시간 스텝 tn에서의 지구-고정 좌표계에 대한 정확한 각위치이다. 3축 가속도계 및 3축 회전 센서를 사용한
Figure pct00131
의 최상의 추정치는
Figure pct00132
로 나타내어진다. 이 추정치는 초기 요각 오프셋을 제외한 짧은 기간에서 높은 정확도를 가진다.
Figure pct00133
Figure pct00134
는 수학식 67로서 표현될 수 있다.
Figure pct00135
제한 없이, 자북이 지구-고정 중력 기준계의 플러스 X 축으로서 사용된다. 수학식 65 내지 수학식 67을 수학식 64에 대입하면, 하기의 수학식이 얻어진다.
Figure pct00136
Figure pct00137
그러면, 문제는
Figure pct00138
및
Figure pct00139
의 행렬이 주어진 경우
Figure pct00140
및
Figure pct00141
을 추정하는 것으로 된다. 간략함을 위해,
Figure pct00142
을 A로 표시하고 C를 수학식 70으로서 정의한다.
Figure pct00143
확장된 Kalman 필터(EKF) 구조체의 6개의 원소는 수학식 71과 같고,
Figure pct00144
여기서,
Figure pct00145
는 벡터 회전을 나타내는 4원수의 스케일 및 벡터 원소이고,
Figure pct00146
는 국소 자계의 경사각이며,
Figure pct00147
는 기준계의 각위치에서의 초기 요각 오프셋이다.
X 및
Figure pct00148
의 초기값은 다음과 같다.
Figure pct00149
Figure pct00150
상태에 대한 과정 모델은 정적이다 - 즉,
Figure pct00151
임 -. 측정 모델은 다음과 같다.
Figure pct00152
예측된 측정치가 수학식 75에 의해 주어진다.
Figure pct00153
상태 X에서의 4원수와 정렬 행렬
Figure pct00154
사이의 관계식이 수학식 76에 의해 주어진다.
Figure pct00155
Figure pct00156
에 대한 A의 편미분은 다음과 같이 주어진다.
Figure pct00157
Figure pct00158
Figure pct00159
Figure pct00160
Figure pct00161
및
Figure pct00162
에 대한 C의 편미분은 다음과 같다.
Figure pct00163
Figure pct00164
G는 다음과 같이 정의된다.
Figure pct00165
원소들이 X에 대한 h의 편미분인 야코비 행렬은 다음과 같다.
Figure pct00166
상태 및 그의 오차 공분산 행렬 갱신에 대해 다음과 같이 표준 EKF 계산 절차가 사용된다.
(1) 오차 공분산 예측
Figure pct00167
(2)혁신 계산
Figure pct00168
수학식 75를 수학식 86에 대입하면, 수학식 87이 얻어진다.
Figure pct00169
(3) Kalman 이득 계산
Figure pct00170
여기서, R은 다음과 같이 주어지는 자력계 측정 잡음 공분산이고:
Figure pct00171
Figure pct00172
(4) 상태 보정
Figure pct00173
(5) 오차 공분산 보정
Figure pct00174
EKF의 표준 절차 이외에, 이 방법은 상태를 제한된 채로 유지하기 위해 2개의 단계를 더 실행하며, 이는 재귀적 필터를 안정화시켜 필터가 발산하는 것을 방지한다.
(6) 4원수 정규화, 회전 행렬을 나타내는 유효한 4원수가 1의 진폭을 가진다.
Figure pct00175
(7) 경사각 및 초기 요각 오프셋에 대한 위상 랩(phase wrap), 유효한 경사각은
Figure pct00176
와
Figure pct00177
사이로 제한되어 있고, 유효한 요각은
Figure pct00178
와
Figure pct00179
사이로 제한되어 있다. 첫째, 경사각 추정치는, 예를 들어, 수학식 93을 사용하여
Figure pct00180
내로 제한되고,
Figure pct00181
여기서,
Figure pct00182
함수는 다음과 같은 일을 한다.
Figure pct00183
코드 1
둘째, 경사각 추정치는 또한
Figure pct00184
내로 제한되는데, 그 이유는 이 동작이 코사인 및 사인의 부호를 변경하기 때문이며, 초기 요각 오프셋에 대한 적절한 변화가 수반될 필요가 있고, 예시적인 코드는 다음과 같다.
Figure pct00185
코드 2
마지막으로, 초기 요각 오프셋 추정치가
Figure pct00186
내로 제한된다.
Figure pct00187
단계 6 및 단계 7이 필요하고 중요하지만, 필터를 안정적으로 유지하는 데 이들 단계로 충분한 것은 아니며 이들 단계가 필터를 보다 빠르게 수렴시키지 않는다.
이 방법에 부가되는 다른 제어 인자는 동적 Q 조정이다. 종래의 방법에서는, Q=0인데, 그 이유는 추정의 상태가 시간에 따라 일정하기 때문이다. 그렇지만, 이것은 데이터 시퀀스가 그다지 익숙하지 않을 때 아주 느린 수렴 속도를 야기한다. 예를 들어, 초기에 수집된 모든 데이터점이 오랫 동안 각위치의 아주 작은 이웃으로부터 온 것인 경우, 이는 궁극적으로 P를 아주 작게 만들 수 있는데, 그 이유는 각각의 시간 스텝이 P를 약간 더 작게 만들기 때문이다. 이어서 추가의 데이터점이 아주 다양한 각위치로부터 아주 짧은 시간 시스템에서 수집될 때, 필터는 아주 작은 P로 인해 그의 상태를 참값으로 신속히 갱신할 수 없다.
이 방법은 영이 아닌 Q를 가능하게 해주고, 이는 필터가 시스템 상태를 적절한 속도로 갱신할 수 있게 해준다. 일반적으로, P가 아주 크게 되어 필터를 불안정하게 만들도록 P를 증가시킬 위험이 존재하지만, 이 방법은 Q를 동적으로 조정하고 따라서 빠르게 수렴하는 이점을 가지며 또한 충분히 안정되도록 해줄 수 있다. 이를 위해, 상수 기준 Q0는 필터가 전체 동적 범위에 대해 할 수 있고 변수가 각각의 시간 스텝에 대해 취할 수 있는 최대 변화로 설정된다.
Figure pct00188
이 방법에서 각각의 시간 스텝에서 최종적인 Q를 조정하기 위해 2개의 동적-변화 곱셈 인자가 사용된다.
Figure pct00189
Figure pct00190
은 현재의 시스템 상태와 정확도 검증 알고리즘으로부터 획득된 시스템 상태 사이의 추정된 오정렬 각도의 차이의 함수이도록 설계된다. 이 차이가 충분히 클 때,
Figure pct00191
은 필터가 그의 최대 수렴 속도로 실행할 수 있게 해준다. 이 차이가 원하는 정확도와 비교하여 충분히 작을 때,
Figure pct00192
은 필터가 느려지게 하고 미세-조정을 수행한다. 예시적인 실시예에서, 이 관계식은 각각의 시간 스텝에서 다음과 같이 구현된다.
Figure pct00193
코드 3
여기서,
Figure pct00194
는 음이 아닌 상수이고 1보다 훨씬 더 작다.
Figure pct00195
는 감쇠 인자(decay factor)이다. 각위치가 고정된 각위치의 이웃에 있을 때,
Figure pct00196
는 지수적으로 감쇠된다. 각위치가 사전 정의된 임계값 ANGLE_TOL보다 더 많이 변할 때,
Figure pct00197
는 다시 1로 점프한다. 이것을 함으로써, 장치가 아주 좁은 각위치 공간 내에 머물러 있을 때 필터가 훨씬 더 큰 P를 갖지 않게 된다. 따라서, 안정성이 보장된다. 2개의 각위치 사이의 차이가 다음과 같이 주어진다.
Figure pct00198
코드 4
여기서 A 및 Aold는, 각각, 2개의 4원수의 방향-코사인 행렬 표현이고, q = dcm2q(dcm)는 방향-코사인 행렬을 4원수 표현으로 변환하는 함수이며, [v, phi] = qdecomp(q)는 단위 벡터 및 4원수의 회전각 성분을 분해하는 함수이다.
Figure pct00199
계산의 예시적인 구현이 다음과 같이 주어진다.
Figure pct00200
코드 5
DECAY_FACTOR는, 예를 들어, 0.95로 설정될 수 있다.
상태가 가장 최근의 측정치로 갱신될 때, 추정된 경사각 및 초기 요각 오프셋이 수학식 97의 최상의 시퀀스를 구성하는 데 사용된다.
Figure pct00201
Figure pct00202
및
Figure pct00203
의 시퀀스 쌍이 주어진 경우,
Figure pct00204
을 푸는 것은 Wahba 문제라고 알려진 것으로 된다. 이 문제를 해결하기 위해 많은 대안의 알고리즘이 개발되었다. 이하에서 단계 1 내지 단계 4로서 기술되어 있는 Landis Markley의 SVD(Singular Value Decomposition) 알고리즘이 여기서 사용된다.
(1) 3x3 행렬 L을 구성한다.
Figure pct00205
(2) 특이값 분해(SVD)를 사용하여 L을 구성한다.
Figure pct00206
(3) 부호를 계산하고
Figure pct00207
를 구성한다.
Figure pct00208
(4) A를 계산한다.
Figure pct00209
A가 계산될 때, 이 방법은 이 A를 상기 EKF의 가장 최근의 상태에서 얻어진 것과 비교하고, 차이의 각도가 코드 4를 사용하여 계산된다. 차이의 각도는 추정된 정렬 행렬의 정확도의 추정치이다. 앞서 언급한 바와 같이, 설계된 EKF에서의 동적 Q 조정에서
Figure pct00210
의 곱셈 인자를 결정하기 위해 차이의 각도가 또한 피드백된다.
보다 쉬운 실시간 구현을 위해, 이력 데이터를 다음과 같이 재귀적으로 저장하기 위해 9개의 1x3 영속 벡터 변수(persistent vector variable)가 사용된다.
Figure pct00211
따라서, 수학식 98이 수학식 103을 사용하여 계산될 수 있다.
Figure pct00212
각위치의 참조된 시퀀스는 임의의 다른 움직임 센서의 결합으로부터, 심지어 다른 자력계로부터 온 것일 수 있다. 이 방법이 3-D 가속도계 및 3-D 회전 센서를 갖는 9축 유형의 센서 유닛인 다른 센서 유닛에 대해 사용될 수 있다. 각위치의 참조된 시퀀스가 다양한 센서-융합 알고리즘을 사용하여 획득될 수 있다.
중력 기준계의 축이 중력 및 자북 방향을 사용하여 위치 확인될 수 있는 한, 지구-고정 중력 기준계가, 중력 및 자북 대신에, 다른 방향을 x-축 및 z-축으로서 갖는 것으로 정의될 수 있다.
참조된 각위치가 미지의 초기 요 오프셋을 갖지 않는 경우,
Figure pct00213
는 참조된 지구-고정 좌표계에 대한 국소 자계의 요각일 수 있고, 수학식 67을 수학식 104로서 다시 쓸 수 있다.
Figure pct00214
이러한 정렬 행렬이 획득된 후에, 국소 자계 벡터가 또한 지구-고정 좌표계에서 자동으로 구해지는데, 그 이유는
Figure pct00215
및
Figure pct00216
가 EKF 상태에서 동시에 구해지기 때문이다.
정렬 알고리즘이 임의의 참조된 장치 몸체에 의한 임의의 센서 3D 정렬을 위해 사용될 수 있고, 자력계 또는 관성 몸체 센서로 제한되지 않는다.
정렬 알고리즘은 하나의 단계에서 이를 풀기 위해 일련의 데이터를 한꺼번에 받을 수 있다.
이 방법은 정확도 검증 알고리즘에 대해 앞서 기술한 것 대신에 Wahba 문제를 풀기 위해 다른 알고리즘을 이용할 수 있다.
그에 부가하여, 각도 차이가 일치성(coincidence)(즉, 해가 향상될 수 없는 동안 루프하는 것)을 피하기 위해 다수의 반복에 대해 사전 결정된 허용 범위보다 작도록 하기 위해 안정성 카운터가 사용될 수 있다.
유사한 결과를 달성하기 위해 EKF의 다른 초기화가 사용될 수 있다. 정렬 추정 알고리즘이 초기화에 민감하지 않다.
상기 예시적인 실시예에서 사용되는 상수가 특정의 목적을 달성하도록 조정될 수 있다. k1 및 k2 값과 그의 적응적 변화 거동이 환경, 센서 및 응용 등에 따라 예시적인 실시예와 상이할 수 있다.
요약하면, 이 섹션에서 기술되는 방법은, 모든 다른 파라미터(스케일, 스큐 및 오프셋)가 이미 사전 교정되어 있거나 다른 방식으로 충분한 정확도로 알려져 있는 한, 임의의 미지의 환경, 미지의 경사각, 및 참조된 자세에서의 미지의 초기 요각 오프셋(총 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는 근방계를 추적하고 보상하는 방법에 관계된 알고리즘을 설명하는 데 사용되는 표기법의 목록이다.
Figure pct00217
Figure pct00218
Figure pct00219
Figure pct00220
지구-고정 중력 기준계에서의 자계가 일정할 때, 장치의 몸체 기준계에서의 자력계에 의해 측정된 자계는 지구-고정 중력 기준계에 대한 장치의 몸체 기준계의 3-D 배향(각위치)을 결정하는 데 사용될 수 있다. 그렇지만, 지구-고정 중력 기준계에서의 자계가 시간에 따라 변할 때, 자력계 측정치가 상당히 변화된다. 이러한 시간 의존적 변화는 강철 효과 또는 연철 효과 등의 소스를 부가하거나 제거하는 이어폰, 스피커, 휴대폰 등의 임의의 근방계 외란으로 인한 것일 수 있다.
자력계가 배향 추정을 위해 또는 나침반용으로 사용될 때 근방계 외란의 존재가 알려져 있지 않은 경우, 추정된 배향 또는 북쪽 방향이 부정확하다. 따라서, 3-D 배향 및 나침반을 결정하기 위해 자력계 측정치를 실제로 사용하기 위해, 근방 자계 추적 및 보상이 바람직하다. 게다가, 3-D 가속도계 및 3-D 회전 센서를 포함하는 결합으로부터 획득된 각위치가 요각 드리프트 문제에 의해 영향을 받는데, 그 이유는 지구-고정 중력 기준계에 대한 장치의 몸체 기준계의 절대 요각의 직접적인 관측이 없기 때문이다. 근방계에 대해 보상되는 자계값은 이 단점을 보정하여, 요각 드리프트 문제를 치유한다.
교정된 자력계(연철 및 강철 효과 교정을 포함함)는 다음을 측정하고:
Figure pct00221
여기서,
Figure pct00222
이고
Figure pct00223
이다.
이 방법은
Figure pct00224
를 동적으로 추적하고 이를 사용하여
Figure pct00225
를 추정하며, 이어서
Figure pct00226
으로부터 이를 보상하여
Figure pct00227
를 획득하고, 추정된
Figure pct00228
는 3-D 배향 측정 및 나침반용으로 사용될 준비가 되어 있다. 방법은 다음과 같은 단계들을 포함할 수 있다.
단계 1: 2개의 영속적 3x1 벡터에서, 동적
Figure pct00229
의 추정치 및 가장 최근의 정상
Figure pct00230
의 추정치 - 각각,
Figure pct00231
및
Figure pct00232
으로 표시되어 있음 - 를 저장한다.
단계 2: 지구-고정 중력 기준계에서의 가상 상수 3x1 벡터를 구성한다.
Figure pct00233
단계 3: 지구-고정 중력 기준계에서의 관측 벡터를 구성한다.
Figure pct00234
각각의 시간 스텝에 대해 하기의 단계들이 실행된다.
단계 4: 기준 배향(각위치)를 사용하여 장치의 몸체 기준계에서의
Figure pct00235
의 표현식을 계산한다.
Figure pct00236
수학식 108에 나타낸 방식으로
Figure pct00237
를 구성함으로써,
Figure pct00238
이
Figure pct00239
에서의 요각 오차에 의해 영향을 받지 않는다.
Figure pct00240
의 z 축의 값이
Figure pct00241
에 대한 벡터
Figure pct00242
의 상대 가중치를 나타내는
Figure pct00243
의 임의의 함수로 설정될 수 있다.
단계 5:
Figure pct00244
와
Figure pct00245
사이의 각도
Figure pct00246
를 계산한다.
단계 6: 지구-고정 중력 기준계에서의 자계(근방계를 포함함)를 예측한다.
Figure pct00247
단계 7: 현재의 자계 추정치와
Figure pct00248
사이의 차이를 계산한다.
Figure pct00249
단계 8: 예를 들어, 단일 지수적 평탄 필터를 사용하여 현재의 자계 추정치를 갱신한다.
Figure pct00250
단계 9:
Figure pct00251
의 총 크기를 계산하고, 그것과
Figure pct00252
의 크기 사이의 차이를 취한다.
Figure pct00253
단계 10:
Figure pct00254
와
Figure pct00255
사이의 각도
Figure pct00256
를 계산한다.
단계 11:
Figure pct00257
와
Figure pct00258
사이의 각도차를 계산한다.
Figure pct00259
단계 12: 예를 들어, 하기의 예시적인 실시예를 사용하여 근방 자계가 안정적인지를 평가한다.
Figure pct00260
코드 6
여기서
Figure pct00261
의 영속적 변수가 근방 자계가 얼마나 오랫동안 변하지 않는지를 기록하는 데 사용된다. 예시적으로,
Figure pct00262
이 3으로 설정될 수 있고,
Figure pct00263
가 4로 설정될 수 있다.
Figure pct00264
가 수학식 116에 의해 주어진다.
Figure pct00265
단계 13:
Figure pct00266
가 사전 정의된 임계값보다 클 때
Figure pct00267
을
Figure pct00268
으로 갱신하고(예컨대, 임계값이 1초와 동등하게 설정될 수 있음) 이어서
Figure pct00269
를 0으로 리셋한다. 단계 13의 예시적인 실시예는 하기의 코드이다.
Figure pct00270
코드 7
단계 14: 예를 들어, 하기의 서브단계를 수행함으로써, 현재의 샘플이 가장 최근의 추정된 정상 자계와 부합하는지를 평가한다.
서브단계 14.1:
Figure pct00271
와
Figure pct00272
사이의 각도차를 계산한다.
Figure pct00273
서브단계 14.2:
Figure pct00274
의 총 크기를 계산하고, 그것과
Figure pct00275
의 크기 사이의 차이를 취한다.
Figure pct00276
서브단계 14.3: 예를 들어, 하기의 코드를 사용하여 14.1 및 14.2에서 계산된 차를 사전 정의된 임계값과 비교한다.
Figure pct00277
예, 현재 샘플이 추정된 정상 근방 자계에 있고,
단계 15 및 단계 16으로 간다.
else
아니오, 단계 15 및 단계 16을 건너뛰고, 현재 샘플이 근방계 보상되지 않고, 배향 추정치 또는 나침반에 대해 주의를 기울일 필요가 있으며, 다음 샘플이 올 때까지 기다린다.
end
코드 8
여기서,
Figure pct00278
및
Figure pct00279
는 보다 많은 샘플이 포함될 수 있게 해주기 위해 타당할 정도로 크도록 설정될 수 있다. 유의할 점은, 코드 8에서의 “else” 단계에 대한 한가지 옵션이 현재의 자계를 더 잘 반영하도록 현재의 모델을 갱신하는 것이라는 것이다.
단계 15: 단계 14의 결과가 현재의 샘플이 가장 최근의 추정된 정상 자계와 부합한다는 것인 경우, 하기의 서브단계를 수행한다.
서브단계 15.1:
Figure pct00280
를 사용하여 지구-고정 중력 기준계에서의 벡터 관측을 구성한다.
Figure pct00281
서브단계 15.2: 장치의 몸체 기준계에서의 벡터 관측을 구성한다.
Figure pct00282
서브단계 15.3: 장치의 몸체 기준계 및 지구-고정 중력 기준계 둘 다에서의 벡터 관측으로 3x3 행렬을 형성한다.
Figure pct00283
서브단계 15.4: 보정된
Figure pct00284
의 해를 구한다. 이 서브단계가 각종의 상이한 알고리즘을 사용하여 구현될 수 있다. 특이값 분해(SVD)법을 사용한 예시적인 실시예가 이하에 기술되어 있다.
(1) SVD를 사용하여 G를 분해한다.
Figure pct00285
(2) 부호를 계산하고
Figure pct00286
를 구성한다.
Figure pct00287
(3)
Figure pct00288
를 계산한다.
Figure pct00289
단계 16: 근방 자계가 보상되어 있는
Figure pct00290
를 계산한다.
Figure pct00291
단계 17:
Figure pct00292
를 사용하여 요각 결정과 연관된 오차를 추정한다.
Figure pct00293
파라미터
Figure pct00294
및
Figure pct00295
가 자력계의 교정의 정확도의 동적 함수로 설정될 수 있다.
최상의 요각 추정치를 얻기 위해 상이한 요각 추정치들을 융합하는 방법.
요각의 잡음있는 추정치들을 융합(즉, 결합)하는 방법이 제공된다. 9축 유형의 장치에서, 하나의 요각 추정치가 교정된 자력계를 사용하여 획득될 수 있고, 다른 단기적으로 안정적이지만 장기적으로 드리프트하는 요각 추정치가 3-D 회전 센서(예컨대, 자이로스코프) 등의 움직임 센서로부터 획득될 수 있다. 이 방법은 요각 오차가 작을 때 평탄한 작은 조정을 가능하게 해주고 요각 오차가 클 때 신속한 큰 조정을 가능하게 해준다. 이하에서 기술되는 방법은 오차가 작을 때는 평탄하게 안정적인 값을 산출하는 요각에 대해 높은 정확도를 달성하면서, 오차가 클 때는 그에 응답하여 빠른 조정을 달성한다. 유의할 점은, 이 동일한 방식이 다른 배향 및 위치 파라미터에도, 특히 피치 및 롤각에 적용될 수 있다는 것이다.
예시적인 실시예에 따르면, 도 12는 최상의 요각 추정치를 얻기 위해 요각 추정치들을 융합시키는 방법(1000)의 블록도이다. 3-D 교정된 자력계로부터의 요각 추정치(1010) 및 몸체 센서(들)로부터의 요각 측정치(1020)가 융합 알고리즘(1030)에 입력된다. 알고리즘(1030)은 최상의 요각 추정치(1040) 및 최상의 요각 추정치(1040)와 연관된 오차(1050)를 출력한다.
최상의 요각 추정치를 획득하기 위해 상이한 요각 추정치들을 융합시키는 방법에 관계된 알고리즘에 대한 이하의 설명에서, 인덱스 n은 시간 스텝 n에서의 값을 나타낸다.
방법의 일부 실시예는 요각 영역에서 동작하는 1차원 적응 필터를 사용한다. 선택적으로, 융합시키는 방법이 수행되어야 하는지(즉, 자력계로부터의 요각 추정치를 유지해야 하는지) 여부를 나타내기 위해 부울 변수(예컨대, “noYawCorrectFromMag_”라고 함)가 사용될 수 있다. 부울 변수의 값이 사전 결정된 조건(들)이 충족되는지에 따라 기본값과 다른 값 사이에서 토글될 수 있다. 방법은 다음과 같은 단계들을 포함할 수 있다:
단계 1: 장치가 움직이지 않고 있는지에 따라 융합이 사용되어야 하는지를 (다양한 방법들 중 하나를 사용하여) 판정한다(예컨대, noYawCorrectFromMag_를 거짓으로 설정함).
단계 2: 몸체 센서를 사용하여 예측된 요각
Figure pct00296
을 획득한다. 예를 들어, 3-D 가속도계 및 3-D 자이로스코프를 몸체 센서로서 사용하여 전체 각위치가 추정될 수 있다.
단계 3: (자북과 중력을 사용하여 지구-고정 중력 기준계를 정의하는 방식에 따라) 교정되고 근방계 보상된 자계 추정치를, 자북과 기준 요-제로(reference yaw-zero) 사이의 상대 초기 요각 오프셋과 함께 사용하여, 요각 추정치
Figure pct00297
을 계산한다.
단계 4: 다음 중 하나 이상을 고려하여 총 추정치 오차
Figure pct00298
를 계산한다:
a. 교정 정확도
b. 센서 잡음, 롤 및 피치 추정치 오차로부터 얻어지는 요각 오차 계산
c. 근방계 보상 오차
단계 5: 단계 2 및 단계 3으로부터의 요각 추정치
Figure pct00299
및
Figure pct00300
을 적응 필터에의 입력으로서 사용하여 적응 필터의 보정 방식을 적용한다. 적응 필터의 출력은 요각의 최상의 추정치
Figure pct00301
이다. 적응 필터의 계수 totalK가 다음의 절차들 중 임의의 절차 또는 그 절차들의 임의의 조합의 생성물을 사용하여 계산될 수 있다.
절차 1: K1은 일반적으로 단계 4에서 계산된 totError
Figure pct00302
에 대한 혁신
Figure pct00303
의 비의 함수이다. 혁신은 자력계로부터의 현재의 요각
Figure pct00304
과 적응 필터의 마지막 상태로부터의 예측된 최상의 요각 추정치
Figure pct00305
사이의 차이다.
Figure pct00306
예시적인 실시예에서, K1은 “totError”
Figure pct00307
에 대한 혁신
Figure pct00308
의 비의 3차 다항식 함수이다.
Figure pct00309
Figure pct00310
여기서 K1은 0과 1 사이로 제한된다.
절차 2: K2는 totError의 제곱
Figure pct00311
에 대한 몸체 센서(예컨대, 자이로스코프)에서의 예측된 요 분산
Figure pct00312
의 비이다.
Figure pct00313
절차 3: “totError”
Figure pct00314
이 임계값
Figure pct00315
이하인 경우, K3은 1이고, 그렇지 않은 경우, 몸체 센서(에컨대, 자이로)의 예측된 요 오차에 대한 혁신의 비의 함수이다. 예를 들어,
Figure pct00316
K3 계산의 예시적인 실시예가 다음과 같이 주어져 있다:
Figure pct00317
코드 9
절차 4: 혁신의 절대값
Figure pct00318
이 임계값
Figure pct00319
보다 큰 경우, K4가 1이고, 그렇지 않은 경우, 0.001과 같은 작은 값의 상수이다.
단계 6: totalK(
Figure pct00320
)을 계산한다. 예를 들어,
Figure pct00321
특정의 조건이 충족되면, totalK는 0으로 설정된다. 이러한 조건은 다음과 같다:
1) 혁신의 절대값
Figure pct00322
이 교정의 정확도보다 작다;
2) 총 추정치 오차 “totError”
Figure pct00323
가 임계값
Figure pct00324
보다 크다;
3) 멤버 변수 noYawCorrectFromMag_가 참이다;
4) IIR 저역 통과 필터링된 버전과 추정된 자계로부터의 측정된 요각의 순간 버전 사이의 차이가 사전 결정된 임계값(예컨대, 0.04 라디안)보다 크다.
최상의 요 추정치가 다음과 같이 계산된다:
Figure pct00325
또는
Figure pct00326
여기서,
Figure pct00327
은
Figure pct00328
의 함수이다. 한 예시적인 실시예에서, 점 [0, 0.002] 및 [4, 1]을 통과하는 비선형 곡선이 사용되고 1에서 포화된다. 다른 예시적인 실시예에서,
Figure pct00329
이다. 자력계로부터의 요각 추정치의 오차가 잘 제한되어 있는 경우, 이는 항상 잘 제한된 정확도를 갖는 요각을 제공하고, 따라서 관성 센서(예컨대, 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 (34)

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

    여기서
    Figure pct00331
    및
    Figure pct00332
    은 틸트 보정된 롤 및 피치이며,
    Figure pct00333
    여기서
    Figure pct00334
    및
    Figure pct00335
    는 상기 요의 원시 추정치를 사용하여 계산된 상기 중력 기준계에서의
    Figure pct00336
    의 성분이고,
    Figure pct00337
    는 상기 추출된 국소 3-D 자계와 중력의 반대 방향 사이의 각도이며,
    Figure pct00338
    는 상기 몸체 기준계에서의 상기 국소 3-D 자계의 추정치이고,
    Figure pct00339
    는 상기 몸체 기준계에서의 상기 국소 3-D 자계의 중력에 평행한 성분의 추정치이며,
    Figure pct00340
    는 상기 몸체 기준계에서의 상기 국소 3-D 자계의 중력에 수직인 성분의 추정치인 방법.
  15. 제1항에 있어서, 상기 적어도 2개의 방법 중 하나가 다음과 같이 상기 요각을 계산하고,
    Figure pct00341

    여기서
    Figure pct00342
    및
    Figure pct00343
    은 틸트 보정된 롤 및 피치이며,
    Figure pct00344
    , 여기서
    Figure pct00345
    및
    Figure pct00346
    는 상기 요의 원시 추정치를 사용하여 계산된 상기 중력 기준계에서의
    Figure pct00347
    의 성분이고,
    Figure pct00348
    는 상기 추출된 국소 3-D 자계와 중력의 반대 방향 사이의 각도이며,
    Figure pct00349
    은 상기 몸체 기준계에서의 상기 국소 3-D 자계의 추정치이고,
    Figure pct00350
    은 상기 몸체 기준계에서의 상기 국소 3-D 자계의 중력에 평행한 성분의 추정치이며,
    Figure pct00351
    은 상기 몸체 기준계에서의 상기 국소 3-D 자계의 중력에 수직인 성분의 추정치인 방법.
  16. 제1항에 있어서, 상기 적어도 2개의 방법 중 하나가 다음과 같이 상기 요각을 계산하고,
    Figure pct00352

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

Applications Claiming Priority (9)

Application Number Priority Date Filing Date Title
US38886510P 2010-10-01 2010-10-01
US61/388,865 2010-10-01
US41457010P 2010-11-17 2010-11-17
US41456010P 2010-11-17 2010-11-17
US41458210P 2010-11-17 2010-11-17
US61/414,570 2010-11-17
US61/414,582 2010-11-17
US61/414,560 2010-11-17
PCT/US2011/054275 WO2012044964A2 (en) 2010-10-01 2011-09-30 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

Publications (1)

Publication Number Publication Date
KR20130143576A true KR20130143576A (ko) 2013-12-31

Family

ID=45893772

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020137011278A Withdrawn KR20130143576A (ko) 2010-10-01 2011-09-30 중력 기준계에서의 장치에 부착된 움직임 센서 및 자력계의 측정치를 사용하여 장치의 요각을 추정하는 장치 및 방법

Country Status (5)

Country Link
US (1) US20130185018A1 (ko)
EP (1) EP2621809A4 (ko)
KR (1) KR20130143576A (ko)
CN (1) CN103153790B (ko)
WO (1) WO2012044964A2 (ko)

Families Citing this family (40)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2703779B1 (en) * 2012-08-29 2020-03-25 BlackBerry Limited Stabilizing Orientation Values Of An Electronic Device
US9310193B2 (en) 2012-08-29 2016-04-12 Blackberry Limited Stabilizing orientation values of an electronic device
US8915116B2 (en) * 2013-01-23 2014-12-23 Freescale Semiconductor, Inc. Systems and method for gyroscope calibration
WO2014151370A1 (en) * 2013-03-15 2014-09-25 Invensense, Inc. Magnetometer using magnetic materials on accelerometer
WO2014179449A1 (en) * 2013-05-01 2014-11-06 Hillcrest Laboratories, Inc. Mapped variable smoothing evolution method and device
US9482554B2 (en) 2013-05-02 2016-11-01 Hillcrest Laboratories, Inc. Gyroscope stabilizer filter
US10261176B2 (en) 2013-05-15 2019-04-16 Flir Systems, Inc. Rotating attitude heading reference systems and methods
WO2016036767A2 (en) * 2014-09-02 2016-03-10 Flir Systems, Inc. Rotating attitude heading reference systems and methods
US10175043B2 (en) 2013-05-15 2019-01-08 FLIR Belgium BVBA Toroidal shape recognition for automatic compass calibration systems and methods
WO2014186636A1 (en) * 2013-05-15 2014-11-20 Flir Systems, Inc. Automatic compass calibration system and corresponding method
US20150019159A1 (en) * 2013-07-15 2015-01-15 Honeywell International Inc. System and method for magnetometer calibration and compensation
EP2883272B1 (en) * 2013-08-27 2016-06-15 CommScope Technologies LLC Alignment determination for antennas and such
CN103619090A (zh) * 2013-10-23 2014-03-05 深迪半导体(上海)有限公司 基于微型惯性传感器舞台灯光自动定位和追踪系统和方法
US10258256B2 (en) 2014-12-09 2019-04-16 TechMah Medical Bone reconstruction and orthopedic implants
GB2524802B (en) * 2014-04-03 2018-11-07 Nokia Technologies Oy A magnetometer apparatus and associated methods
US10274318B1 (en) * 2014-09-30 2019-04-30 Amazon Technologies, Inc. Nine-axis quaternion sensor fusion using modified kalman filter
CN107003144B (zh) * 2014-11-11 2020-11-03 英特尔公司 基于扩展卡尔曼滤波器的自动磁强计校准
CN105352487B (zh) * 2015-10-13 2018-06-15 上海华测导航技术股份有限公司 一种姿态测量系统的精度校准方法
EP3795109B1 (en) * 2015-12-16 2022-09-14 TechMah Medical LLC A method of calibrating an inertial measurement unit
WO2017158633A1 (en) * 2016-03-17 2017-09-21 Gipstech S.R.L. Method for estimating the direction of motion of an individual
CN106326576B (zh) * 2016-08-26 2019-07-12 北京控制工程研究所 一种任意基准系下的整星偏置角动量的偏航估计方法
CN113804191B (zh) 2016-11-17 2024-03-19 格兰菲智能科技有限公司 移动设备及求取移动设备姿态的方法
TWI608320B (zh) * 2016-12-19 2017-12-11 四零四科技股份有限公司 三維軌跡驗證裝置及其方法
US10983206B2 (en) 2017-11-07 2021-04-20 FLIR Belgium BVBA Low cost high precision GNSS systems and methods
US11280896B2 (en) 2017-06-16 2022-03-22 FLIR Belgium BVBA Doppler GNSS systems and methods
RU2653967C1 (ru) * 2017-06-20 2018-05-15 Федеральное государственное бюджетное образовательное учреждение высшего образования "Саратовский государственный технический университет имени Гагарина Ю.А." (СГТУ имени Гагарина Ю.А.) Способ автономной ориентации подвижных объектов
CN107830861A (zh) * 2017-12-07 2018-03-23 智灵飞（北京）科技有限公司 基于自适应增益互补滤波运动物体姿态测量方法及装置
US12044533B2 (en) 2018-02-28 2024-07-23 Ceva Technologies, Inc. Methods and apparatus for planar magnetometer calibration, heading determination, gyroscope assisted magnetometer amplitude calibration, magnetometer amplitude and alignment calibration, magnetometer mapping, and sensor fusion
CN109260647A (zh) * 2018-09-10 2019-01-25 郑州大学 基于多模态信号的人体跳跃指标综合评测及训练系统
US12050114B2 (en) * 2020-03-09 2024-07-30 Deere & Company Method and system for detection of roll sensor bias
FI3926298T3 (fi) 2020-06-17 2025-05-19 Eta Sa Mft Horlogere Suisse Navigointilaite kallistumankorjauksella ja siihen liittyvä menetelmä
CN112902828B (zh) * 2021-01-19 2023-09-08 陕西福音假肢有限责任公司 一种角度计算方法
CN112904884B (zh) * 2021-01-28 2023-01-24 歌尔股份有限公司 足式机器人轨迹跟踪方法、设备及可读存储介质
DE102021109700A1 (de) 2021-04-16 2022-10-20 Airbus Operations Gmbh Flugzustandsermittlungsvorrichtung und verfahren zum ermitteln eines flugzustandes eines flugzeugs
CN113281824B (zh) * 2021-05-19 2022-03-25 北京大学 一种考虑飞机非刚性以及极化电流因素的航空磁补偿方法
CN113335411A (zh) * 2021-07-15 2021-09-03 福建工程学院 核电站磁吸爬壁机器人及其控制系统和方法
US12284058B2 (en) * 2022-06-16 2025-04-22 Samsung Electronics Co., Ltd. Self-tuning fixed-point least-squares solver
CN116817896B (zh) * 2023-04-03 2024-04-16 盐城数智科技有限公司 一种基于扩展卡尔曼滤波的姿态解算方法
CN116772903B (zh) * 2023-08-16 2023-10-20 河海大学 基于迭代ekf的sins/usbl安装角估计方法
CN121208967B (zh) * 2025-11-28 2026-01-30 中震华创(深圳)技术有限公司 一种设备姿态自动矫正方法、装置及光终端烈度仪

Family Cites Families (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5645077A (en) * 1994-06-16 1997-07-08 Massachusetts Institute Of Technology Inertial orientation tracker apparatus having automatic drift compensation for tracking human head and other similarly sized body
US6421622B1 (en) * 1998-06-05 2002-07-16 Crossbow Technology, Inc. Dynamic attitude measurement sensor and method
US20030158699A1 (en) * 1998-12-09 2003-08-21 Christopher P. Townsend Orientation sensor
US6725173B2 (en) * 2000-09-02 2004-04-20 American Gnc Corporation Digital signal processing method and system thereof for precision orientation measurements
US6834436B2 (en) * 2001-02-23 2004-12-28 Microstrain, Inc. Posture and body movement measuring system
US6860023B2 (en) * 2002-12-30 2005-03-01 Honeywell International Inc. Methods and apparatus for automatic magnetic compensation
KR100594971B1 (ko) * 2004-01-09 2006-06-30 삼성전자주식회사 지자기 센서를 이용한 입력장치 및 이를 이용한 입력신호생성방법
KR100574506B1 (ko) * 2004-02-26 2006-04-27 삼성전자주식회사 연산된 방위각의 오류여부를 표시하는 지자기센서 및 그방위각측정방법
KR100611182B1 (ko) * 2004-02-27 2006-08-10 삼성전자주식회사 회전상태에 따라 메뉴표시상태를 변경하는 휴대형전자기기 및 그 방법
CN101256456B (zh) * 2004-04-30 2015-12-02 希尔克瑞斯特实验室公司 具有倾斜补偿和改进的可用性的自由空间定位方法及装置
WO2005108119A2 (en) * 2004-04-30 2005-11-17 Hillcrest Laboratories, Inc. Free space pointing devices with tilt compensation and improved usability
EP1872087A4 (en) * 2005-04-19 2012-10-17 Jaymart Sensors Llc MINIATURED INERTIA MEASURING UNIT AND ASSOCIATED PROCEDURES
US8239162B2 (en) * 2006-04-13 2012-08-07 Tanenhaus & Associates, Inc. Miniaturized inertial measurement unit and associated methods
US20070088496A1 (en) * 2005-07-20 2007-04-19 Atair Aerospace, Inc. Automatic heading and reference system
KR100655937B1 (ko) * 2005-11-25 2006-12-11 삼성전자주식회사 방위각을 산출하는 지자기 센서 및 그 방법
FR2933212B1 (fr) * 2008-06-27 2013-07-05 Movea Sa Pointeur a capture de mouvement resolue par fusion de donnees

Also Published As

Publication number Publication date
EP2621809A4 (en) 2017-12-06
CN103153790A (zh) 2013-06-12
WO2012044964A3 (en) 2012-07-26
WO2012044964A2 (en) 2012-04-05
CN103153790B (zh) 2016-06-08
US20130185018A1 (en) 2013-07-18
EP2621809A2 (en) 2013-08-07

Similar Documents

Publication Publication Date Title
KR20130143576A (ko) 중력 기준계에서의 장치에 부착된 움직임 센서 및 자력계의 측정치를 사용하여 장치의 요각을 추정하는 장치 및 방법
US10670624B2 (en) Real-time accelerometer calibration
CN103941309B (zh) 地磁传感器校准设备及其方法
US10685083B2 (en) Apparatuses and methods for calibrating magnetometer attitude-independent parameters
CN106647791B (zh) 三维姿态测控装置、机械设备及三维姿态的测控方法
US10209078B2 (en) Local perturbation rejection using time shifting
CN103299247B (zh) 用于磁近场的动态追踪及补偿的设备和方法
US20130245984A1 (en) Apparatuses and methods for magnetometer alignment calibration without prior knowledge of the local magnetic field
Wahdan et al. Magnetometer calibration for portable navigation devices in vehicles using a fast and autonomous technique
CN103017763B (zh) 状态估计设备和偏移更新方法
JP6894593B2 (ja) 差動測定による磁石の位置の追跡方法
CN119195746A (zh) 基于高精度补偿算法的井斜方位测量系统
US11815568B2 (en) System and method for fast magnetometer calibration using gyroscope
KR101462007B1 (ko) 자세 추정 장치 및 자세 추정 방법
Allotta et al. Underwater Vehicles attitude estimation in presence of magnetic disturbances
JP2013072643A (ja) 磁気測定データ校正装置及び方位角計測装置
JP6097500B2 (ja) 磁気測定データ校正装置及び方位角計測装置
US10444030B1 (en) Automatic calibration of magnetic sensors based on optical image tracking
JP2013088162A (ja) 状態推定装置
Asher et al. IMU Filter
CN107144271A (zh) 数据处理方法和装置

Legal Events

Date Code Title Description
PA0105 International application

St.27 status event code: A-0-1-A10-A15-nap-PA0105

PG1501 Laying open of application

St.27 status event code: A-1-1-Q10-Q12-nap-PG1501

P11-X000 Amendment of application requested

St.27 status event code: A-2-2-P10-P11-nap-X000

P13-X000 Application amended

St.27 status event code: A-2-2-P10-P13-nap-X000

A201 Request for examination
P11-X000 Amendment of application requested

St.27 status event code: A-2-2-P10-P11-nap-X000

P13-X000 Application amended

St.27 status event code: A-2-2-P10-P13-nap-X000

PA0201 Request for examination

St.27 status event code: A-1-2-D10-D11-exm-PA0201

E902 Notification of reason for refusal
PE0902 Notice of grounds for rejection

St.27 status event code: A-1-2-D10-D21-exm-PE0902

T11-X000 Administrative time limit extension requested

St.27 status event code: U-3-3-T10-T11-oth-X000

N231 Notification of change of applicant
PN2301 Change of applicant

St.27 status event code: A-3-3-R10-R13-asn-PN2301

St.27 status event code: A-3-3-R10-R11-asn-PN2301

P11-X000 Amendment of application requested

St.27 status event code: A-2-2-P10-P11-nap-X000

P13-X000 Application amended

St.27 status event code: A-2-2-P10-P13-nap-X000

PC1202 Submission of document of withdrawal before decision of registration

St.27 status event code: N-1-6-B10-B11-nap-PC1202

WITB Written withdrawal of application
P22-X000 Classification modified

St.27 status event code: A-2-2-P10-P22-nap-X000

P22-X000 Classification modified

St.27 status event code: A-2-2-P10-P22-nap-X000