KR20110056185A - 리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치 - Google Patents

리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치 Download PDF

Info

Publication number
KR20110056185A
KR20110056185A KR1020090117306A KR20090117306A KR20110056185A KR 20110056185 A KR20110056185 A KR 20110056185A KR 1020090117306 A KR1020090117306 A KR 1020090117306A KR 20090117306 A KR20090117306 A KR 20090117306A KR 20110056185 A KR20110056185 A KR 20110056185A
Authority
KR
South Korea
Prior art keywords
value
matrix
rotation
rotation error
error matrix
Prior art date
Application number
KR1020090117306A
Other languages
English (en)
Other versions
KR101140379B1 (ko
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 KR20110056185A publication Critical patent/KR20110056185A/ko
Application granted granted Critical
Publication of KR101140379B1 publication Critical patent/KR101140379B1/ko

Links

Images

Classifications

    • 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/08—Control of attitude, i.e. control of roll, pitch, or yaw

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

본 발명의 일 측면에 따르면, 리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치가 제공된다.
관성 측정 장치, 자세 추정, 지수 사상, 리 대수, 칼만 필터

Description

리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치{METHOD AND APPARATUS FOR ESTIMATING ORIENTATION BY USING LIE ALGEBRA AND KALMAN FILTER}
본 발명은 물체의 자세(orientation) 추정 기술에 관한 것으로, 특히 리 대수(Lie algebra)를 칼만 필터(Kalman filter)에 적용하여 물체의 자세를 추정하는 기술에 관한 것이다.
자세 추정(orientation estimation)은 비행 항법 장치, 잠수함 제어 및 안정화 장치, 증강 또는 혼합 현실 제공 장치, 로봇 제어 장치 등 다양한 기술 분야에 널리 적용되며, 이를 위한 다양한 방법이 현재 제공되고 있다. 이러한 방법들로 오일러 각도(Euler angle)를 사용하는 방법, 단위 사원수(unit quaternion)를 칼만 필터(Kalman filter)와 함께 사용하는 방법 등이 있다. 그러나 오일러 각도를 사용하는 방법은 특이점(singularity)으로 인하여 강건한 자세 추정이 불가능한 경우가 종종 발생하고, 단위 사원수를 칼만 필터와 함께 사용하는 방법은 단위 사원수를 재정규화(renormalization)해야 하는 등, 종래 방법들은 여러 문제점을 지니고 있다.
본 발명은 상기 문제점을 해결하기 위한 것으로, 리 대수를 칼만 필터에 적용하여 주변 자기장 변화와 같은 외란 뿐만 아니라 물체의 움직임이 큰 경우에도 자세를 강건하게 추정할 수 있도록 하는 물체의 자세 추정 방법 및 장치를 제공하는 것을 그 목적으로 가진다.
본 발명의 일 측면에 따르면, 가속도 센서, 자기장 센서 및 자이로 센서를 포함하는 관성 측정 장치를 이용하여 물체의 자세를 추정하는 방법이 제공된다. 관성 측정 장치는 물체에 부착되고, 상기 방법은, 자이로 센서에서 제공되는 각속도 값에 기초하여 물체의 자세를 나타내는 물체의 회전 행렬을 예측(predict)하는 단계, 가속도 센서 및 자기장 센서에서 각각 제공되는 중력 값 및 자기장 값으로부터 예측된 회전 행렬의 예측 오차를 나타내는 회전 에러 행렬을 생성하는 단계, 회전 에러 행렬의 리 대수(Lie algebra) 값을 생성하는 단계 및 회전 에러 행렬의 리 대수 값에 기초하여 칼만 필터 알고리즘에 따라 예측된 회전 행렬을 교정(correct)하는 단계를 포함한다.
상기와 같은 본 발명에 따르면, 물체의 자세를 강건하게 추정할 수 있도록 하는 방법 및 장치를 제공할 수 있다.
이하, 본 명세서의 일부를 이루는 첨부 도면이 참조된다. 도면에 있어서, 유사한 부호들은 문맥상 달리 해석되지 않는 이상 유사한 구성을 나타낸다. 상세한 설명, 도면 및 청구항에 서술된 예시적인 실시예들은 제한적인 의도로 사용된 것이 아니다. 본 명세서에 나타난 대상의 사상 또는 범위에 벗어나지 않으면서, 다른 실시예들이 사용될 수 있으며, 다른 변경이 이루어질 수 있다. 본 명세서에 일반적으로 설명되고, 도면에 예시된 본 개시의 구성들은 다른 다양한 구성들로 배치되거나, 대체되거나 결합될 수 있음이 용이하게 이해될 것이며, 그 모든 것들은 명백하게 고려되고, 본 명세서의 일부를 구성한다.
본 발명의 일 실시예에 따른 관성 측정 장치는 물체에 부착(예를 들어, 항공기에 설치되거나, 무인 항공기, 무인 자동차, 혹은 카메라에 부착)되어 물체의 자세와 관련된 각종 측정값들을 제공하고, 회전 행렬을 생성하기 위한 컴퓨팅 장치(예를 들어, 관성 센서와 같은 회전 행렬 생성 장치)는 이들 측정값들에 기초하여 물체의 자세를 나타내는 회전 행렬을 생성할 수 있다. 관성 측정 장치와 컴퓨팅 장치는 하나의 케이스 내에 하나의 장치로 구현되거나, 아니면 서로 독립적인 장치로서 유, 무선 연결수단을 통하여 연결되도록 구성될 수 있다.
도 1은 본 발명의 일 실시예에 따른 자세 추정 장치의 블록도이다. 도 1을 참조하면, 자세 추정 장치(100)는 임의의 물체(예를 들어, 항공기)에 부착되어 해당 물체의 자세(orientation)을 추정하는 장치로서, 다양한 종류의 측정값을 제공하는 관성 측정부(110)와 위 측정값들에 기초하여 물체의 자세를 나타내는 회전 행렬을 생성하는 회전 행렬 생성부(120)를 포함할 수 있다.
도 2는 본 발명의 일 실시예에 따른 물체의 회전 행렬을 설명하기 위한 개념도이다. 도 2를 참조하면, 회전 행렬은 임의의 기준 좌표계(reference frame) 또는 항법 좌표계(nagivation frame) {N}(201)에 대한 물체(200)의 물체 좌표계(body fixed frame) {B}(202)의 상대적인 자세를 나타내는 데이터로서, 물체의 회전행렬을 구하면 항법 좌표계에 대한 해당 물체의 상대적인 자세를 알 수 있다. 이러한 회전 행렬들의 집합은 행렬 리 군(matrix Lie group)의 하위 군(sub-group)인 특수 직교 군(special orthogonal group: SO)를 형성한다. 회전 행렬이 도 2에 도시된 바와 같이 3개의 축으로 이루어진 3차원 좌표계에 대한 회전 행렬인 경우에는 이러한 회전행렬들의 집합은 아래와 같이 정의되는 3차원 특수 직교 군 SO(3)일 수 있다.
Figure 112009073939600-PAT00001
여기서, R은 회전행렬, I는 단위행렬, 그리고 det R은 회전행렬의 행렬식을 나타낸다.
도 1을 다시 참조하면, 관성 측정부(110)는 자이로 센서(111), 가속도 센서(112) 및 자기장 센서(113)(또는 컴파스 센서)를 포함할 수 있다. 자이로 센서(111)(예를 들어, 3축 자이로 센서)는 물체의 각속도를 측정하는 센서로서, 물체의 각속도 값에 소정의 바이어스 값이 더해진 값을 그 측정값으로 제공할 수 있다. 가속도 센서(112)(예를 들어, 3축 가속도 센서)는 물체의 선형 가속도를 측정하기 위한 센서로서, 물체에 가해지는 중력 값과 순수한 물체 가속도 값을 더한 값을 그 측정값으로 제공할 수 있다. 자기장 센서(113)(예를 들어, 3축 자기장 센서)는 물 체에 가해지는 지구자기장을 측정하기 위한 센서로서, 물체에 가해지는 지구 자기장 값에 다른 주변의 자기장에 의한 외란 값들이 더해진 값을 그 측정값으로 제공할 수 있다.
회전 행렬 생성부(120)는 자이로 센서(111)에서 제공되는 측정값 중 각속도 값을 추출하고, 추출된 각속도 값에 기초하여 물체의 회전 행렬을 예측하고, 가속도 센서(112) 및 자기장 센서(113)에서 제공하는 측정값들 중 중력 값 및 자기장 값에 기초하여 예측된 회전 행렬을 교정할 수 있다. 물체의 각속도는 물체가 회전하는 속도를 나타내는 것으로서 측정된 각속도 값들을 적분하면 물체의 자세를 구할 수 있고, 측정된 각속도 값들에 비추어 보아 물체의 향후 자세 또는 회전 행렬을 예측할 수 있으나, 다양한 요인들로 인하여 위와 같은 예측은 부정확할 수 있다. 따라서, 위와 같은 예측이 이루어진 이후 제공되는 측정값들 중 중력 값 및 자기장 값에 기초하여 예측된 회전 행렬을 교정할 수 있다.
일 실시예에 있어서, 회전 행렬 생성부(120)는 교정 동작을 위해서, 중력 값 및 자기장 값에 기초하여 예측된 회전 행렬의 예측 오차를 나타내는 회전 에러 행렬을 구하고, 회전 에러 행렬에 기초하여 예측된 회전 행렬을 교정하는 동작을 수행할 수 있다. 일 실시예에 있어서, 회전 행렬 생성부(120)는 예측된 회전 행렬을 교정하는 동작을 회전 에러 행렬의 리 대수 값과 이를 매개화하는 벡터 값에 기초하여 칼만 필터 알고리즘에 따라 수행할 수 있다. 예를 들어, 회전 행렬 생성부(120)는 (a) 리 군(특수 직교 군)의 원소인 회전 에러 행렬의 리 대수 값을 생성하고, (b) 회전 에러 행렬의 리 대수 값을 매개화하는 벡터 값을 생성하고, (c) 벡 터 값을 칼만 필터 알고리즘의 상태(state)로 설정하고 칼만 필터 알고리즘을 수행하여 교정된 벡터 값을 구하고, (d) 교정된 벡터 값에 대응되는, 교정된 회전 에러 행렬을 구하고, (e) 교정된 회전 에러 행렬에 기초하여 예측된 회전 행렬을 교정할 수 있다.
리 대수는 리 군(예를 들어, SO(3))에 지수 사상(exponential mapping)을 수행하여 구할 수 있는 집합으로서, 리 대수의 원소인 리 대수 값은 벡터로 매개화하여 표현할 수 있다. 예를 들어, 회전 행렬 또는 회전 에러 행렬이 3개의 축으로 이루어진 3차원 좌표계에 대한 것인 경우, 이러한 회전행렬들 또는 회전 에러 행렬들의 집합 SO(3)에 지수 사상을 수행하면 아래와 같은 리 대수 so(3)로서, 3x3 교대행렬 A의 집합
Figure 112009073939600-PAT00002
을 구할 수 있다. 그리고, 이러한 3x3 교대행렬 A(=[w])는 3개의 변수가 행렬 내의 정해진 위치에 배치되는 구조를 갖기 때문에 교대행렬 [w] 대신에 아래와 같이 3개의 변수만으로 이루어진 벡터 vex([w])로 매개화하여 나타낼 수 있다.
Figure 112009073939600-PAT00003
칼만 필터 알고리즘은 소정의 측정값에 따라 소정의 예측된 물리적 상태(state)를 교정하고, 다시 교정된 상태에 기초하여 물리적 상태를 예측하는 동작들을 반복하는 재귀적 알고리즘으로서, 이러한 칼만 필터 알고리즘의 상태로는 벡터가 사용되는 것이 바람직하다. 단위 사원수 등 벡터와 다른 구조를 갖는 데이터 가 사용되는 경우에는 칼만 필터 알고리즘을 수행하는 도중에 계속적으로 칼만 필터 알고리즘의 상태를 재정규화해야 하는 등 다양한 문제점들이 나타날 수 있다. 이와 관련하여, 본 실시예에서 교정하고자 하는 회전 행렬은 도 2와 관련하여 설명한 바와 같이 리 군의 원소이나, 이러한 리 군의 원소는 벡터가 아니며, 예측된 회전 행렬의 예측 오차를 나타내는 회전 에러 행렬 또한 리 군의 원소로서 벡터가 아니다.
상술한 실시예에 있어서, 회전 행렬 생성부(120)는 회전 에러 행렬을 칼만 필터 알고리즘의 상태로 놓는 대신에, 칼만 필터 알고리즘에 적합한 구조를 갖는 벡터 값(즉, 회전 에러 행렬의 리 대수 값을 매개화한 벡터 값)을 칼만 필터 알고리즘의 상태로 놓고 교정 동작을 수행하기 때문에, 보다 효율적으로 회전 행렬의 교정 동작을 수행할 수 있다.
도 3은 도 1에 도시된 회전 행렬 생성부의 상세 블록도이다. 도 3을 참조하면, 회전 행렬 생성부(120)는 회전 행렬 초기화부(310), 바이어스 설정부(320), 회전 행렬 예측부(330), 회전 추정 행렬 생성부(340), 회전 에러 행렬 생성부(350) 및 회전 행렬 교정부(360)를 포함할 수 있다.
회전 행렬 초기화부(310)는 회전 행렬 생성부(120)가 회전행렬을 예측 및 교정하는데 필요한 각종 변수들을 초기화할 수 있다. 일 실시예에 있어서, 회전 행렬 초기화부(310)는 회전 행렬 교정부(360)가 회전 에러 행렬을 교정하는데 필요한 칼만 필터 알고리즘의 프로세스 에러 공분산 값 및 측정 에러 공분산 값을 주어진 초기값으로 초기화할 수 있다.
바이어스 설정부(320)는 자이로 센서(111), 가속도 센서(112) 및 자기장 센서(113)가 제공하는 측정값들에 기초하여 자이로 센서(111)의 바이어스 값을 생성한다. 일 실시예에 있어서, 바이어스 설정부(320)는 위 측정값들에 기초하여 위 측정값들이 물체가 정지상태에서 측정된 값들인지 여부를 판단하고, 정지상태에서 측정된 값들인 경우, 자이로 센서(111)에서 제공한 측정값을 바이어스 값으로 설정할 수 있다. 바이어스 설정부(320)는 또한 새로이 제공된 측정값들이 정지상태에서 측정된 값들로 판단되는 경우에는, 새로이 제공된 측정값들 중 자이로 센서(111)에서 제공한 측정값을 바이어스 값으로 갱신할 수 있다.
회전 행렬 예측부(330)는 자이로 센서(111)에서 제공되는 측정값에서 상기 설정된 바이어스 값을 차감하여 각속도 값을 생성하고, 생성된 각속도 값으로부터 각속도 교대행렬을 생성한다. 회전 행렬 예측부(330)는 생성된 각속도 교대행렬에 기초하여 향후 회전 행렬을 예측할 수 있다.
회전 추정 행렬 생성부(340)는 회전 행렬 예측부(330)가 회전 행렬을 예측한 이후에 가속도 센서(112) 및 자기장 센서(113)로부터 제공되는 측정값들에 기초하여 현재 회전 행렬을 추정하는 회전 추정 행렬을 생성할 수 있다. 일 실시예에 있어서, 회전 추정 행렬 생성부(340)는 TRIAD 알고리즘을 사용하여 회전 추정 행렬을 생성할 수 있다. TRIAD 알고리즘 등 회전 추정 행렬을 생성하는 알고리즘은 당업계에 널리 알려져 있는바 자세한 설명은 생략한다.
회전 에러 행렬 생성부(350)는 회전 행렬 예측부(330)에 의해 예측된 회전 행렬과 회전 추정 행렬 생성부(340)에 의해 생성된 회전 추정 행렬에 기초하여 예 측된 회전 행렬과 회전 추정 행렬 간의 차를 나타내는 회전 에러 행렬을 생성할 수 있다. 회전 에러 행렬 생성부(350)는 생성된 회전 에러 행렬의 리 대수 값을 구하고, 상기 리 대수 값을 매개화하는 벡터 값을 생성할 수 있다. 일 실시예에 있어서, 회전 에러 행렬 생성부(350)는 회전 에러 행렬에 지수 사상을 수행하여 회전 에러 행렬의 리 대수 값을 생성할 수 있다. 위 실시예에 있어서, 회전 에러 행렬 생성부(350)는 회전 에러 행렬의 로그 값을 상기 회전 에러 행렬의 리 대수로서 생성할 수 있다.
회전 행렬 교정부(360)는 회전 에러 행렬 생성부(350)에 의해 생성된 벡터 값을 칼만 필터 알고리즘의 상태로 설정하고, 위 상태에 칼만 필터 알고리즘을 수행하여 회전 에러 행렬의 리 대수 값을 교정할 수 있다. 회전 행렬 교정부(360)는 교정된 회전 에러 행렬의 리 대수 값에 기초하여 회전 에러 행렬을 교정하고, 교정된 회전 에러 행렬에 기초하여 상기 예측된 회전 행렬을 교정할 수 있다.
이하, 도 3에 도시된 회전 행렬 생성부(120)의 각 구성요소들이 수행하는 동작들의 구체적인 내용에 대하여 수식들을 사용하여 자세히 설명한다.
정의 및 표기법
항법 좌표계는 {N}으로 표기되고, 물체에 고정된 자세 추정 장치의 물체 좌표계는 {B}로 표기된다. 회전 행렬 R은 RN B로서 {B}의 {N}에 대한 상대적인 자세를 나타내고,
Figure 112009073939600-PAT00004
는 R의 예측치 및 교정치를 나타낸다. wb는 {B}를 기준으로 측정된 각속도를 나타낸다.
회전 행렬의 지수 사상
G가 일반 행렬 리 군을 나타내고, g가 그것의 리 대수를 나타낼 때, 지수 사상 exp:g->G는 지수 행렬(matrix exponential)로 정의된다. so(3)의 원소의 매개화된 벡터
Figure 112009073939600-PAT00005
로 표현될 때, Rodrigue의 공식에 따르면 so(3)의 원소 [s]는 아래와 같은 공식으로 나타낼 수 있다.
Figure 112009073939600-PAT00006
위에서 α 및 β는 θ가 0 근방인 값인 경우에도 잘 정의(well-defined)되어 있으며, 테일러 급수 확장을 사용하여 그 값들을 정확하게 구할 수 있다. 주어진 회전 행렬 R(즉, SO(3))마다 유일한 [s](즉, so(3))가 존재한다. 위 [s]의 크기는 구간 [0, π)내에 있으며, exp([s])=R의 관계가 성립한다.
θ가 π와 다른 경우에는 아래와 같은 식이 성립한다.
Figure 112009073939600-PAT00007
여기서, rij는 회전 행렬의 (i,j)번째 요소를 나타내고, rij=[R]ij로도 표시된다.
θ가 π에 근접한 값인 경우에는 s가 다른 방식으로 계산되어야 한다. θ가 π에 근접한 값인 경우에는 테일러 급수를 이용하여 아래와 같은 식으로 나타낼 수 있다.
Figure 112009073939600-PAT00008
SO(3)를 사용한 자세 추정 방법
1. 센서 모델링
물체의 자세는 그것의 물체 좌표계 {B}와 항법 좌표계 {N}간의 관계를 특정하여 구할 수 있다. 가속도 센서 및 자기장 센서는 긴 주기를 갖는 측정값들을 제공하고, 자이로 센서는 짧은 주기를 갖는 측정값들을 제공한다. 자이로 센서가 측정한 각속도 값들은 회전 행렬을 예측하기 위한 프로세스 모델의 입력으로 사용된다. 가속도 센서 및 자기장 센서의 측정값들은 예측된 회전 행렬을 교정하는데 사용된다.
물체는 다음과 같은 운동 방정식에 따라 그 자세가 변화한다.
Figure 112009073939600-PAT00009
자이로 센서의 천천히 변화하는 바이어스 벡터 b는 백색 가우시안 잡음 벡터 ε에 의해 구동되는 1차 마르코프 프로세스의 실현(realization)으로 모사된다.
Figure 112009073939600-PAT00010
일 실시예에 있어서, 칼만 필터의 상태는 회전 에러 행렬의 리 대수 값을 매개화하는 벡터 값을 포함할 수 있다. 일 실시예에 있어서, 칼만 필터의 상태는 다음과 같은 벡터로 기술될 수 있다.
Figure 112009073939600-PAT00011
2. 프로세스 모델 - 회전 행렬의 예측
본 알고리즘의 프로세스 모델은 다음과 같이 기술될 수 있다.
Figure 112009073939600-PAT00012
여기서, R-는 예측된 회전행렬, R+는 이전에 교정된 회전 행렬 또는 초기화된 회전 행렬, h는 샘플링 시간을 나타낸다.
칼만필터의 에러 상태는
Figure 112009073939600-PAT00013
와 같이 계산될 수 있다.
Figure 112009073939600-PAT00014
는 추정된 회전행렬과 측정된 행렬 간의 회전에러행렬을 나타낸다. 에러 상태의 상태 천이는 다음과 같은 공식에 의해 계산될 수 있다.
Figure 112009073939600-PAT00015
여기서 vk는 백색 가우시안 잡음을 나타낸다. 에러 상태의 추정값과 에러 공분산 P의 추정값은 다음과 같은 공식에 의해 계산된다.
Figure 112009073939600-PAT00016
Figure 112009073939600-PAT00017
3. 측정 모델
자세를 연산하는데 필요한 최소 측정 벡터 개수는 두 개이다. TRIAD 알고리 즘은 Wahba의 문제를 해결하기 위한 단일 좌표계의 확정적 방법이다. TRIAD 알고리즘을 사용하면 가속도 센서 및 자기장 센서의 정규화된 측정값들로부터 회전추정행렬 Yk +1을 구할 수 있다. 회전추정행렬 Yk +1은 {B}의 {N}에 대한 상대적인 자세를 나타낸다. Yk+1 및 이전 예측된 회전행렬 R-에 기초해서
Figure 112009073939600-PAT00018
의 측정값 y를 다음과 같이 구할 수 있다.
Figure 112009073939600-PAT00019
Figure 112009073939600-PAT00020
에러 상태의 칼만 이득 K는 다음과 같이 계산된다.
Figure 112009073939600-PAT00021
Figure 112009073939600-PAT00022
Figure 112009073939600-PAT00023
위 공식 중 칼만이득 Kk +1에 대한 공식 중 Rk +1은 회전 행렬이 아니라 측정 잡음의 공분산을 나타낸다.
추정된 SO(3) 행렬은 so(3)인
Figure 112009073939600-PAT00024
의 지수와 곱해져서 갱신된다.
Figure 112009073939600-PAT00025
중력 및 자기장 벡터의 측정값들은 {B}에 대한 고정 관성 방향에서의 측정값 들이다.
도 4는 본 발명의 일 실시예에 따른 자세 추정 방법의 절차도이다. 도 4를 참조하면, 자세 추정 장치의 자세 회전 행렬 초기화부는 자세 추정 장치가 회전행렬을 예측 및 교정하는데 필요한 각종 변수들을 초기화할 수 있다(단계 410). 단계 420에서, 자세 추정 장치의 바이어스 설정부는 자세 추정 장치의 관성 측정부(예를 들어, 자이로 센서, 가속도 센서 및 자기장 센서)가 제공하는 측정값들에 기초하여 자이로 센서의 바이어스 값을 생성한다. 단계 430에서, 자세 추정 장치의 회전 행렬 예측부는 자이로 센서에서 제공되는 측정값에서 상기 설정된 바이어스 값을 차감하여 각속도 값을 생성하고, 생성된 각속도 값으로부터 향후 회전 행렬을 예측한다. 단계 440에서, 자세 추정 장치의 회전 추정 행렬 생성부는 회전 행렬 예측부가 회전 행렬을 예측한 이후에 가속도 센서 및 자기장 센서로부터 제공되는 측정값들에 기초하여 현재 회전 행렬을 추정하는 회전 추정 행렬을 생성한다. 단계 450에서, 자세 추정 장치의 회전 에러 행렬 생성부는 회전 행렬 예측부에 의해 예측된 회전 행렬과 회전 추정 행렬 생성부에 의해 생성된 회전 추정 행렬에 기초하여 예측된 회전 행렬과 회전 추정 행렬 간의 차를 나타내는 회전 에러 행렬을 생성한다. 단계 460에서, 자세 추정 장치의 회전 행렬 교정부는 회전 에러 행렬 생성부에 의해 생성된 벡터 값을 칼만 필터 알고리즘의 상태로 설정하고, 위 상태에 칼만 필터 알고리즘을 수행하여 회전 에러 행렬의 리 대수 값을 교정하고, 회전 행렬 교정부는 교정된 회전 에러 행렬에 기초하여 상기 예측된 회전 행렬을 교정한다. 단계 470에서, 자세 추정 장치는 관성 측정부에서 추가 측정값들이 제공되는 지 여부를 판단하고, 제공되는 경우 단계 420으로 이동하고, 제공되지 않는 경우 절차를 종료한다.
도 5는 본 발명의 일 실시예에 따른 바이어스 값 설정 및 갱신 방법의 절차도이다. 도 5를 참조하면, 자세 추정 장치의 바이어스 설정부는 자세 추정 장치의 가속도 센서, 자기장 센서 및 자이로 센서로부터 각각 측정값을 입력받는다(단계 510). 단계 520에서, 바이어스 설정부는 가속도 센서의 측정값이 지구 중력 값을 포함하는 제1 범위 내에 있는지 여부를 판단한다. 제1 범위 내에 있는 경우, 단계 530에서, 바이어스 설정부는 가속도 센서의 측정값과 상기 자기장 센서의 측정값 간의 내적 값이 제2 범위 내에 있는지 여부를 판단한다. 제2 범위 내에 있는 경우, 단계 540에서, 바이어스 설정부는 소정의 시간에 걸쳐 상기 자이로 센서에서 제공한 측정값들이 모두 제3 범위 내에 있는지 여부를 판단한다. 모두 제3 범위 내에 있는 경우, 바이어스 설정부는 자이로 센서의 측정값을 바이어스 값으로 설정 또는 갱신하고, 절차를 종료한다. 반면에, 제1 범위 내에 있지 않거나, 제2 범위 내에 있지 않거나, 제3 범위 내에 있지 않는 경우, 절차를 종료한다.
당업자는 본 명세서에 상술된 및 다른 프로세스들, 상호작용들 및 방법들에 있어서, 프로세스들, 상호 작용들 및 방법들 내에서 수행되는 기능들은 상이한 순서로 구현될 수 있다는 것을 알 것이다. 이에 더하여, 설명된 단계들 및 동작들은 예시로만 제공된다. 즉, 단계들 및 동작들 중 일부는 선택적일 수 있으며, 개시된 실시예들의 본질을 훼손하지 않으면서, 보다 적은 단계들 및 동작들로 합쳐지거나, 추가적인 단계들 및 동작들로 확장될 수 있다.
당업자는 또한 본 명세서에 설명된 장치들 및 방법들은 하드웨어, 소프트웨어, 펌웨어, 미들웨어 또는 이들의 조합들로 구현될 수 있고, 시스템들, 서브시스템들, 구성품들 또는 그들의 서브 구성품들에 사용될 수 있다는 것을 알 것이다. 예를 들어, 소프트웨어로 구현된 방법은 방법의 동작들을 수행하기 위한 컴퓨터 코드를 포함할 수 있다. 이러한 컴퓨터 코드는 프로세서 판독 가능 매체 또는 컴퓨터 프로그램 제품과 같은 기계로 판독 가능한 매체에 저장되거나, 반송파, 또는 반송파에 의해 변조된 신호로 구현되는 컴퓨터 데이터 신호로서 전송 매체 또는 통신 링크를 통해 전송될 수 있다. 기계로 판독 가능한 매체 또는 프로세서로 판독 가능한 매체는 기계(예를 들어, 프로세서, 컴퓨터 등)에 의해 판독되고 실행될 수 있는 형태로 정보를 저장 또는 전달할 수 있는 임의의 매체를 포함할 수 있다.
상술된 사항으로부터, 본 명세서의 다양한 실시예들은 예시를 목적으로 설명되었으며, 본 명세서의 범위 및 취지를 벗어나지 않으면서 다양한 변형된 형태로 구현할 수 있다. 따라서, 본 명세서에 기술된 다양한 실시예들은 제한적인 의미를 가지지 않으며, 본 명세서의 진정한 범위 및 취지는 후술되는 특허청구범위에 의해 나타내어진다.
도 1은 본 발명의 일 실시예에 따른 자세 추정 장치의 블록도.
도 2는 본 발명의 일 실시예에 따른 물체의 회전 행렬을 설명하기 위한 개념도.
도 3은 도 1에 도시된 회전 행렬 생성부의 상세 블록도.
도 4는 본 발명의 일 실시예에 따른 자세 추정 방법의 절차도.
도 5는 본 발명의 일 실시예에 따른 바이어스 값 설정 및 갱신 방법의 절차도.

Claims (16)

  1. 물체에 부착된 관성 측정 장치로부터 제공되는 상기 물체에 대한 각속도 값, 상기 물체가 받는 중력 값 및 상기 물체에서의 자기장 값에 기초하여 컴퓨팅 장치에서 상기 물체의 자세를 추정하는 방법으로서,
    상기 각속도 값에 기초하여 상기 물체의 회전 행렬 - 상기 회전 행렬은 상기 물체의 자세를 나타냄 - 을 예측(predict)하는 단계;
    상기 중력 값 및 상기 자기장 값으로부터 회전 에러 행렬 - 상기 회전 에러 행렬은 상기 예측된 회전 행렬의 예측 오차를 나타냄 - 을 생성하는 단계;
    상기 회전 에러 행렬의 리 대수(Lie algebra) 값을 생성하는 단계; 및
    상기 회전 에러 행렬의 리 대수 값에 기초하여 칼만 필터 알고리즘에 따라 상기 예측된 회전 행렬을 교정(correct)하는 단계
    를 포함하는 방법.
  2. 제1항에 있어서, 상기 예측된 회전 행렬을 교정하는 단계는,
    상기 회전 에러 행렬의 리 대수 값을 매개화하는 벡터 값을 생성하는 단계;
    상기 벡터 값을 상기 칼만 필터 알고리즘의 상태(state)로 제공하는 단계;
    상기 제공된 상태에 기초하여 상기 칼만 필터 알고리즘을 수행하여 상기 벡터 값을 교정하는 단계; 및
    상기 교정된 벡터 값에 기초하여 상기 예측된 회전 행렬을 교정하는 단계
    를 포함하는 방법.
  3. 제1항에 있어서, 상기 회전 에러 행렬의 리 대수 값을 생성하는 단계는,
    상기 회전 에러 행렬에 지수 사상(exponential mapping)을 수행하여 상기 회전 에러 행렬의 리 대수를 생성하는 단계
    를 포함하는, 방법.
  4. 제3항에 있어서, 상기 회전 에러 행렬의 리 대수를 생성하는 단계는,
    상기 회전 에러 행렬의 로그 값을 상기 회전 에러 행렬의 리 대수로서 생성하는 단계
    를 포함하는, 방법.
  5. 제1항에 있어서, 상기 회전 에러 행렬을 생성하는 단계는,
    상기 중력 값 및 상기 자기장 값에 기초하여 상기 회전 추정 행렬을 생성하는 단계;
    상기 예측된 회전 행렬과 상기 회전 추정 행렬에 기초하여 상기 회전 에러 행렬을 생성하는 단계 - 상기 회전 에러 행렬은 상기 예측된 회전 행렬과 상기 회전 추정 행렬 간의 차를 나타냄 -
    를 포함하는 방법.
  6. 제1항에 있어서, 상기 물체의 회전 행렬을 예측하는 단계는,
    상기 자이로 센서의 바이어스 값을 설정하는 단계;
    상기 자이로 센서로부터 측정값을 제공받는 단계; 및
    상기 측정값에서 상기 바이어스 값을 차감한 값을 상기 각속도 값으로서 생성하는 단계
    를 포함하는 방법.
  7. 제6항에 있어서, 상기 자이로 센서의 바이어스 값을 설정하는 단계는,
    상기 가속도 센서, 상기 자기장 센서 및 상기 자이로 센서로부터 각각 측정값을 제공받는 단계;
    상기 가속도 센서의 측정값이 지구 중력 값을 포함하는 제1 범위 내에 있는지 여부를 판단하는 단계;
    상기 가속도 센서의 측정값과 상기 자기장 센서의 측정값 간의 내적 값이 제2 범위 내에 있는지 여부를 판단하는 단계; 및
    상기 가속도 센서의 측정값이 상기 제1 구간 내에 있고, 상기 내적 값이 상기 제2 구간 내에 있는 경우, 상기 자이로 센서의 측정값을 바이어스 값으로 설정하는 단계
    를 포함하는 방법.
  8. 제7항에 있어서, 상기 자이로 센서의 바이어스 값을 설정하는 단계는,
    소정의 시간에 걸쳐 상기 자이로 센서에서 제공한 측정값들이 모두 제3 범위 내에 있는지 여부를 판단하는 단계
    를 더 포함하는 방법.
  9. 물체의 자세를 추정하는 장치로서,
    상기 물체에 부착되는 관성 측정부 - 상기 관성 측정부는 상기 물체에 대한 각속도 값, 상기 물체가 받는 중력 값 및 상기 물체에서의 자기장 값을 제공함 - ;
    상기 각속도 값에 기초하여 상기 물체의 회전 행렬 - 상기 회전 행렬은 상기 물체의 자세를 나타냄 - 을 예측(predict)하고, 상기 중력 값 및 상기 자기장 값으로부터 회전 에러 행렬 - 상기 회전 에러 행렬은 상기 예측된 회전 행렬의 예측 오차를 나타냄 -- 생성하고, 상기 회전 에러 행렬의 리 대수(Lie algebra) 값을 생성하고, 상기 회전 에러 행렬의 리 대수 값에 기초하여 칼만 필터 알고리즘에 따라 상기 예측된 회전 행렬을 교정(correct)하는 회전 행렬 생성부
    를 포함하는 장치.
  10. 제9항에 있어서, 상기 회전 행렬 생성부는 상기 회전 에러 행렬의 리 대수 값을 매개화하는 벡터를 상기 칼만 필터 알고리즘의 상태(state)로 제공하고, 상기 제공된 상태에 기초하여 상기 칼만 필터 알고리즘을 수행하여 상기 벡터를 교정하고, 상기 교정된 벡터에 기초하여 상기 예측된 회전 행렬을 교정하는, 장치.
  11. 제9항에 있어서, 상기 회전 행렬 생성부는 상기 회전 에러 행렬에 지수 사상(exponential mapping)을 수행하여 상기 회전 에러 행렬의 리 대수를 생성하는, 장치.
  12. 제9항에 있어서, 상기 회전 행렬 생성부는 상기 회전 에러 행렬의 로그 값을 상기 회전 에러 행렬의 리 대수로서 생성하는, 장치.
  13. 제9항에 있어서, 상기 회전 행렬 생성부는 상기 중력 값 및 상기 자기장 값에 기초하여 상기 회전 추정 행렬을 생성하고, 상기 예측된 회전 행렬과 상기 회전 추정 행렬에 기초하여 상기 회전 에러 행렬을 생성하고, 상기 회전 에러 행렬은 상기 예측된 회전 행렬과 상기 회전 추정 행렬 간의 차를 나타내는, 장치.
  14. 제9항에 있어서, 상기 회전 행렬 생성부는 상기 자이로 센서의 바이어스 값을 설정하고, 상기 자이로 센서로부터 측정값을 제공받고, 상기 측정값에서 상기 바이어스 값을 차감한 값을 상기 각속도 값으로서 생성하고, 상기 각속도 값에 지수 사상을 수행하여 상기 각속도 값의 리 대수를 생성하는, 장치.
  15. 제14항에 있어서, 상기 회전 행렬 생성부는 상기 가속도 센서, 상기 자기장 센서 및 상기 자이로 센서로부터 각각 측정값을 제공받고, 상기 가속도 센서의 측정값이 지구 중력 값을 포함하는 제1 범위 내에 있는지 여부를 판단하고, 상기 가 속도 센서의 측정값과 상기 자기장 센서의 측정값 간의 내적 값이 제2 범위 내에 있는지 여부를 판단하고, 상기 가속도 센서의 측정값이 상기 제1 구간 내에 있고, 상기 내적 값이 상기 제2 구간 내에 있는 경우, 상기 자이로 센서의 측정값을 바이어스 값으로 설정하는, 장치.
  16. 제15항에 있어서, 상기 회전 행렬 생성부는 소정의 시간에 걸쳐 상기 자이로 센서에서 제공한 측정값들이 모두 제3 범위 내에 있는지 여부를 판단하는, 장치.
KR1020090117306A 2009-11-20 2009-11-30 리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치 KR101140379B1 (ko)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
KR20090112826 2009-11-20
KR1020090112826 2009-11-20

Publications (2)

Publication Number Publication Date
KR20110056185A true KR20110056185A (ko) 2011-05-26
KR101140379B1 KR101140379B1 (ko) 2012-05-03

Family

ID=44364790

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020090117306A KR101140379B1 (ko) 2009-11-20 2009-11-30 리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치

Country Status (1)

Country Link
KR (1) KR101140379B1 (ko)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101462007B1 (ko) * 2013-08-22 2014-11-18 한국과학기술연구원 자세 추정 장치 및 자세 추정 방법
US9167440B2 (en) 2011-12-07 2015-10-20 Samsung Electronics Co., Ltd. Mobile terminal device for positioning system based on magnetic field map and method thereof
CN114459507A (zh) * 2022-03-03 2022-05-10 湖南大学无锡智能控制研究院 一种dvl安装误差标定方法、设备及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4952518B2 (ja) 2007-11-05 2012-06-13 株式会社デンソー 車両用現在位置検出装置及びプログラム

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9167440B2 (en) 2011-12-07 2015-10-20 Samsung Electronics Co., Ltd. Mobile terminal device for positioning system based on magnetic field map and method thereof
US9453932B2 (en) 2011-12-07 2016-09-27 Samsung Electronics Co., Ltd. Mobile terminal device for positioning system based on magnetic field map and method thereof
KR101462007B1 (ko) * 2013-08-22 2014-11-18 한국과학기술연구원 자세 추정 장치 및 자세 추정 방법
CN114459507A (zh) * 2022-03-03 2022-05-10 湖南大学无锡智能控制研究院 一种dvl安装误差标定方法、设备及系统
CN114459507B (zh) * 2022-03-03 2024-02-09 湖南大学无锡智能控制研究院 一种dvl安装误差标定方法、设备及系统

Also Published As

Publication number Publication date
KR101140379B1 (ko) 2012-05-03

Similar Documents

Publication Publication Date Title
Ghobadi et al. Robust attitude estimation from uncertain observations of inertial sensors using covariance inflated multiplicative extended Kalman filter
JP5237723B2 (ja) 動的に較正されるセンサデータと、ナビゲーションシステム内の繰り返し拡張カルマンフィルタとを使用する、ジャイロコンパスの整合用のシステム及び方法
US9417091B2 (en) System and method for determining and correcting field sensors errors
CN106500693B (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
JP4876204B2 (ja) 小型姿勢センサ
JP2014089113A (ja) 姿勢推定装置及びプログラム
KR101922700B1 (ko) 가속도 센서와 지자기 센서 기반의 각속도 산출 방법 및 장치
KR20140025319A (ko) 근 자기장의 동적 추적 및 보상을 위한 장치 및 방법
JP2012173190A (ja) 測位システム、測位方法
CN112798021B (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
KR20060048581A (ko) 네비게이션 중복성을 위한 시스템
CN108318027B (zh) 载体的姿态数据的确定方法和装置
Troni et al. Adaptive Estimation of Measurement Bias in Three-Dimensional Field Sensors with Angular Rate Sensors: Theory and Comparative Experimental Evaluation.
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
JP2015179002A (ja) 姿勢推定方法、姿勢推定装置及びプログラム
JP2019120587A (ja) 測位システム及び測位方法
Troni et al. Experimental evaluation of a MEMS inertial measurements unit for Doppler navigation of underwater vehicles
KR101140379B1 (ko) 리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치
Lee et al. Interference-compensating magnetometer calibration with estimated measurement noise covariance for application to small-sized UAVs
JP2013122384A (ja) カルマンフィルタ、及び、状態推定装置
CN114964214B (zh) 一种航姿参考系统的扩展卡尔曼滤波姿态解算方法
Munguia et al. An attitude and heading reference system (AHRS) based in a dual filter
CN110375773B (zh) Mems惯导系统姿态初始化方法
JP2006038650A (ja) 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム
KR101941009B1 (ko) 자세추정 시스템 및 자세 추정 시스템을 포함하는 무인 이동 장치

Legal Events

Date Code Title Description
A201 Request for examination
E902 Notification of reason for refusal
E902 Notification of reason for refusal
E701 Decision to grant or registration of patent right
GRNT Written decision to grant
FPAY Annual fee payment

Payment date: 20160401

Year of fee payment: 5