KR101140379B1 - Method and apparatus for estimating orientation by using lie algebra and kalman filter - Google Patents

Method and apparatus for estimating orientation by using lie algebra and kalman filter Download PDF

Info

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

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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

본 발명의 일 측면에 따르면, 리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치가 제공된다.According to an aspect of the present invention, a method and apparatus for attitude estimation using a logarithmic and Kalman filter are provided.

관성 측정 장치, 자세 추정, 지수 사상, 리 대수, 칼만 필터 Inertial Measurement Unit, Attitude Estimation, Exponential Mapping, Li Algebra, Kalman Filter

Description

리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치{METHOD AND APPARATUS FOR ESTIMATING ORIENTATION BY USING LIE ALGEBRA AND KALMAN FILTER}Posture estimation method and apparatus using a logarithmic and Kalman filter {METHOD AND APPARATUS FOR ESTIMATING ORIENTATION BY USING LIE ALGEBRA AND KALMAN FILTER}

본 발명은 물체의 자세(orientation) 추정 기술에 관한 것으로, 특히 리 대수(Lie algebra)를 칼만 필터(Kalman filter)에 적용하여 물체의 자세를 추정하는 기술에 관한 것이다.The present invention relates to a technique for estimating the orientation of an object, and more particularly, to a technique for estimating the pose of an object by applying a Li algebra to a Kalman filter.

자세 추정(orientation estimation)은 비행 항법 장치, 잠수함 제어 및 안정화 장치, 증강 또는 혼합 현실 제공 장치, 로봇 제어 장치 등 다양한 기술 분야에 널리 적용되며, 이를 위한 다양한 방법이 현재 제공되고 있다. 이러한 방법들로 오일러 각도(Euler angle)를 사용하는 방법, 단위 사원수(unit quaternion)를 칼만 필터(Kalman filter)와 함께 사용하는 방법 등이 있다. 그러나 오일러 각도를 사용하는 방법은 특이점(singularity)으로 인하여 강건한 자세 추정이 불가능한 경우가 종종 발생하고, 단위 사원수를 칼만 필터와 함께 사용하는 방법은 단위 사원수를 재정규화(renormalization)해야 하는 등, 종래 방법들은 여러 문제점을 지니고 있다.Orientation estimation is widely applied to various technical fields such as flight navigation apparatus, submarine control and stabilization apparatus, augmented or mixed reality providing apparatus, and robot control apparatus, and various methods for this are currently provided. These methods include the use of Euler angles and the use of unit quaternions with the Kalman filter. However, the use of Euler angles often makes robust pose estimation impossible due to singularity, and the use of unit employee numbers with the Kalman filter requires renormalization of the unit employee. Conventional methods have several problems.

본 발명은 상기 문제점을 해결하기 위한 것으로, 리 대수를 칼만 필터에 적용하여 주변 자기장 변화와 같은 외란 뿐만 아니라 물체의 움직임이 큰 경우에도 자세를 강건하게 추정할 수 있도록 하는 물체의 자세 추정 방법 및 장치를 제공하는 것을 그 목적으로 가진다. The present invention is to solve the above problems, by applying a logarithmic number to the Kalman filter, a method and apparatus for estimating the attitude of the object so that it is possible to robustly estimate the posture even when the movement of the object is large, as well as disturbances such as changes in the surrounding magnetic field To provide for that purpose.

본 발명의 일 측면에 따르면, 가속도 센서, 자기장 센서 및 자이로 센서를 포함하는 관성 측정 장치를 이용하여 물체의 자세를 추정하는 방법이 제공된다. 관성 측정 장치는 물체에 부착되고, 상기 방법은, 자이로 센서에서 제공되는 각속도 값에 기초하여 물체의 자세를 나타내는 물체의 회전 행렬을 예측(predict)하는 단계, 가속도 센서 및 자기장 센서에서 각각 제공되는 중력 값 및 자기장 값으로부터 예측된 회전 행렬의 예측 오차를 나타내는 회전 에러 행렬을 생성하는 단계, 회전 에러 행렬의 리 대수(Lie algebra) 값을 생성하는 단계 및 회전 에러 행렬의 리 대수 값에 기초하여 칼만 필터 알고리즘에 따라 예측된 회전 행렬을 교정(correct)하는 단계를 포함한다.According to an aspect of the present invention, there is provided a method for estimating the pose of an object using an inertial measurement device including an acceleration sensor, a magnetic field sensor, and a gyro sensor. An inertial measurement device is attached to the object, the method comprising predicting a rotation matrix of the object representing the pose of the object based on the angular velocity value provided by the gyro sensor, the gravity provided by the acceleration sensor and the magnetic field sensor respectively. Generating a rotation error matrix representing a predicted error of the predicted rotation matrix from the values and magnetic field values, generating a Li algebra value of the rotation error matrix, and a Kalman filter based on the Li algebraic value of the rotation error matrix Correcting the predicted rotation matrix according to the algorithm.

상기와 같은 본 발명에 따르면, 물체의 자세를 강건하게 추정할 수 있도록 하는 방법 및 장치를 제공할 수 있다.According to the present invention as described above, it is possible to provide a method and apparatus for robustly estimating the pose of an object.

이하, 본 명세서의 일부를 이루는 첨부 도면이 참조된다. 도면에 있어서, 유사한 부호들은 문맥상 달리 해석되지 않는 이상 유사한 구성을 나타낸다. 상세한 설명, 도면 및 청구항에 서술된 예시적인 실시예들은 제한적인 의도로 사용된 것이 아니다. 본 명세서에 나타난 대상의 사상 또는 범위에 벗어나지 않으면서, 다른 실시예들이 사용될 수 있으며, 다른 변경이 이루어질 수 있다. 본 명세서에 일반적으로 설명되고, 도면에 예시된 본 개시의 구성들은 다른 다양한 구성들로 배치되거나, 대체되거나 결합될 수 있음이 용이하게 이해될 것이며, 그 모든 것들은 명백하게 고려되고, 본 명세서의 일부를 구성한다.Reference is now made to the accompanying drawings, which form a part of this specification. In the drawings, like numerals refer to similar constructions unless the context clearly indicates otherwise. The illustrative embodiments described in the detailed description, drawings, and claims are not meant to be limiting. Other embodiments may be utilized, and other changes may be made, without departing from the spirit or scope of the subject matter presented herein. It will be readily understood that the configurations of the present disclosure generally described herein and illustrated in the drawings may be arranged, replaced, or combined in various other configurations, all of which are expressly contemplated, and part of this specification Configure.

본 발명의 일 실시예에 따른 관성 측정 장치는 물체에 부착(예를 들어, 항공기에 설치되거나, 무인 항공기, 무인 자동차, 혹은 카메라에 부착)되어 물체의 자세와 관련된 각종 측정값들을 제공하고, 회전 행렬을 생성하기 위한 컴퓨팅 장치(예를 들어, 관성 센서와 같은 회전 행렬 생성 장치)는 이들 측정값들에 기초하여 물체의 자세를 나타내는 회전 행렬을 생성할 수 있다. 관성 측정 장치와 컴퓨팅 장치는 하나의 케이스 내에 하나의 장치로 구현되거나, 아니면 서로 독립적인 장치로서 유, 무선 연결수단을 통하여 연결되도록 구성될 수 있다. An inertial measurement apparatus according to an embodiment of the present invention is attached to an object (eg, installed in an aircraft, attached to an unmanned aerial vehicle, an unmanned vehicle, or a camera) to provide various measurement values related to the attitude of the object, and to rotate the object. A computing device for generating a matrix (eg, a rotation matrix generating device such as an inertial sensor) may generate a rotation matrix representing the pose of the object based on these measurements. The inertial measurement device and the computing device may be implemented as one device in one case or may be configured to be connected via wired or wireless connection as independent devices.

도 1은 본 발명의 일 실시예에 따른 자세 추정 장치의 블록도이다. 도 1을 참조하면, 자세 추정 장치(100)는 임의의 물체(예를 들어, 항공기)에 부착되어 해당 물체의 자세(orientation)을 추정하는 장치로서, 다양한 종류의 측정값을 제공하는 관성 측정부(110)와 위 측정값들에 기초하여 물체의 자세를 나타내는 회전 행렬을 생성하는 회전 행렬 생성부(120)를 포함할 수 있다. 1 is a block diagram of an attitude estimation apparatus according to an embodiment of the present invention. Referring to FIG. 1, the attitude estimating apparatus 100 is an apparatus for estimating an orientation of a corresponding object attached to an arbitrary object (for example, an aircraft), and provides an inertial measurement unit that provides various types of measurement values. The rotation matrix generator 120 may generate a rotation matrix indicating the attitude of the object based on the reference numeral 110 and the above measured values.

도 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)일 수 있다.2 is a conceptual diagram illustrating a rotation matrix of an object according to an embodiment of the present invention. Referring to FIG. 2, the rotation matrix is a body fixed frame {B} 202 of the object 200 with respect to an arbitrary reference frame or navigation frame {N} 201. As a data representing the relative pose of, we can find the relative pose of the object with respect to the navigation coordinate system by obtaining the rotation matrix of the object. These sets of rotation matrices form a special orthogonal group (SO) that is a sub-group of the matrix Lie group. When the rotation matrix is a rotation matrix for a three-dimensional coordinate system composed of three axes as shown in FIG. 2, the set of rotation matrices may be a three-dimensional special orthogonal group SO (3) defined as follows.

Figure 112009073939600-pat00001
Figure 112009073939600-pat00001

여기서, R은 회전행렬, I는 단위행렬, 그리고 det R은 회전행렬의 행렬식을 나타낸다. Where R is the rotation matrix, I is the unit matrix, and det R is the determinant of the rotation matrix.

도 1을 다시 참조하면, 관성 측정부(110)는 자이로 센서(111), 가속도 센서(112) 및 자기장 센서(113)(또는 컴파스 센서)를 포함할 수 있다. 자이로 센서(111)(예를 들어, 3축 자이로 센서)는 물체의 각속도를 측정하는 센서로서, 물체의 각속도 값에 소정의 바이어스 값이 더해진 값을 그 측정값으로 제공할 수 있다. 가속도 센서(112)(예를 들어, 3축 가속도 센서)는 물체의 선형 가속도를 측정하기 위한 센서로서, 물체에 가해지는 중력 값과 순수한 물체 가속도 값을 더한 값을 그 측정값으로 제공할 수 있다. 자기장 센서(113)(예를 들어, 3축 자기장 센서)는 물 체에 가해지는 지구자기장을 측정하기 위한 센서로서, 물체에 가해지는 지구 자기장 값에 다른 주변의 자기장에 의한 외란 값들이 더해진 값을 그 측정값으로 제공할 수 있다.Referring back to FIG. 1, the inertial measurement unit 110 may include a gyro sensor 111, an acceleration sensor 112, and a magnetic field sensor 113 (or a compass sensor). The gyro sensor 111 (eg, a three-axis gyro sensor) is a sensor for measuring the angular velocity of an object, and may provide a value obtained by adding a predetermined bias value to the angular velocity value of the object as the measured value. The acceleration sensor 112 (for example, the 3-axis acceleration sensor) is a sensor for measuring linear acceleration of an object, and may provide a value obtained by adding a gravity value applied to an object and a pure object acceleration value. . The magnetic field sensor 113 (for example, the 3-axis magnetic field sensor) is a sensor for measuring the earth's magnetic field applied to an object. The measured value can be provided.

회전 행렬 생성부(120)는 자이로 센서(111)에서 제공되는 측정값 중 각속도 값을 추출하고, 추출된 각속도 값에 기초하여 물체의 회전 행렬을 예측하고, 가속도 센서(112) 및 자기장 센서(113)에서 제공하는 측정값들 중 중력 값 및 자기장 값에 기초하여 예측된 회전 행렬을 교정할 수 있다. 물체의 각속도는 물체가 회전하는 속도를 나타내는 것으로서 측정된 각속도 값들을 적분하면 물체의 자세를 구할 수 있고, 측정된 각속도 값들에 비추어 보아 물체의 향후 자세 또는 회전 행렬을 예측할 수 있으나, 다양한 요인들로 인하여 위와 같은 예측은 부정확할 수 있다. 따라서, 위와 같은 예측이 이루어진 이후 제공되는 측정값들 중 중력 값 및 자기장 값에 기초하여 예측된 회전 행렬을 교정할 수 있다.The rotation matrix generator 120 extracts the angular velocity value from the measured values provided by the gyro sensor 111, predicts the rotation matrix of the object based on the extracted angular velocity value, and the acceleration sensor 112 and the magnetic field sensor 113. The predicted rotation matrix can be corrected based on the gravity value and the magnetic field value among the measured values provided in The angular velocity of the object indicates the speed at which the object rotates. The integrated angular velocity values can be used to determine the pose of the object. The angular velocity can be used to predict the future pose or rotation matrix of the object. As a result, such predictions may be inaccurate. Therefore, the predicted rotation matrix may be corrected based on the gravity value and the magnetic field value among the measured values provided after the above prediction is made.

일 실시예에 있어서, 회전 행렬 생성부(120)는 교정 동작을 위해서, 중력 값 및 자기장 값에 기초하여 예측된 회전 행렬의 예측 오차를 나타내는 회전 에러 행렬을 구하고, 회전 에러 행렬에 기초하여 예측된 회전 행렬을 교정하는 동작을 수행할 수 있다. 일 실시예에 있어서, 회전 행렬 생성부(120)는 예측된 회전 행렬을 교정하는 동작을 회전 에러 행렬의 리 대수 값과 이를 매개화하는 벡터 값에 기초하여 칼만 필터 알고리즘에 따라 수행할 수 있다. 예를 들어, 회전 행렬 생성부(120)는 (a) 리 군(특수 직교 군)의 원소인 회전 에러 행렬의 리 대수 값을 생성하고, (b) 회전 에러 행렬의 리 대수 값을 매개화하는 벡터 값을 생성하고, (c) 벡 터 값을 칼만 필터 알고리즘의 상태(state)로 설정하고 칼만 필터 알고리즘을 수행하여 교정된 벡터 값을 구하고, (d) 교정된 벡터 값에 대응되는, 교정된 회전 에러 행렬을 구하고, (e) 교정된 회전 에러 행렬에 기초하여 예측된 회전 행렬을 교정할 수 있다. In one embodiment, the rotation matrix generation unit 120 obtains a rotation error matrix indicating a prediction error of the rotation matrix predicted based on the gravity value and the magnetic field value for the calibration operation, and predicted based on the rotation error matrix. The operation of correcting the rotation matrix may be performed. In one embodiment, the rotation matrix generator 120 may perform an operation of correcting the predicted rotation matrix according to the Kalman filter algorithm based on the logarithmic value of the rotation error matrix and a vector value that mediates it. For example, the rotation matrix generator 120 generates (a) the logarithm of the logarithm of the rotation error matrix that is an element of the group Lee (special orthogonal group), and (b) mediates the logarithm of the logarithm of the rotation error matrix. Generate a vector value, (c) set the vector value to the state of the Kalman filter algorithm, perform a Kalman filter algorithm to obtain a calibrated vector value, and (d) correspond to the calibrated vector value Obtain a rotation error matrix and (e) correct the predicted rotation matrix based on the corrected rotation error matrix.

리 대수는 리 군(예를 들어, SO(3))에 지수 사상(exponential mapping)을 수행하여 구할 수 있는 집합으로서, 리 대수의 원소인 리 대수 값은 벡터로 매개화하여 표현할 수 있다. 예를 들어, 회전 행렬 또는 회전 에러 행렬이 3개의 축으로 이루어진 3차원 좌표계에 대한 것인 경우, 이러한 회전행렬들 또는 회전 에러 행렬들의 집합 SO(3)에 지수 사상을 수행하면 아래와 같은 리 대수 so(3)로서, 3x3 교대행렬 A의 집합

Figure 112009073939600-pat00002
을 구할 수 있다. 그리고, 이러한 3x3 교대행렬 A(=[w])는 3개의 변수가 행렬 내의 정해진 위치에 배치되는 구조를 갖기 때문에 교대행렬 [w] 대신에 아래와 같이 3개의 변수만으로 이루어진 벡터 vex([w])로 매개화하여 나타낼 수 있다.Li algebra is a set that can be obtained by performing exponential mapping on a Li group (for example, SO (3)). The Li algebraic value, which is an element of Li algebra, can be expressed by mediating a vector. For example, if the rotation matrix or the rotation error matrix is for a three-dimensional coordinate system consisting of three axes, the exponential mapping on the set of rotation matrices or rotation error matrices SO (3) gives (3), a set of 3x3 alternating matrix A
Figure 112009073939600-pat00002
Can be obtained. In addition, since the 3x3 alternating matrix A (= [w]) has a structure in which three variables are arranged at predetermined positions in the matrix, instead of the alternating matrix [w], a vector vex ([w]) consisting of only three variables is shown below. It can be represented by mediating.

Figure 112009073939600-pat00003
Figure 112009073939600-pat00003

칼만 필터 알고리즘은 소정의 측정값에 따라 소정의 예측된 물리적 상태(state)를 교정하고, 다시 교정된 상태에 기초하여 물리적 상태를 예측하는 동작들을 반복하는 재귀적 알고리즘으로서, 이러한 칼만 필터 알고리즘의 상태로는 벡터가 사용되는 것이 바람직하다. 단위 사원수 등 벡터와 다른 구조를 갖는 데이터 가 사용되는 경우에는 칼만 필터 알고리즘을 수행하는 도중에 계속적으로 칼만 필터 알고리즘의 상태를 재정규화해야 하는 등 다양한 문제점들이 나타날 수 있다. 이와 관련하여, 본 실시예에서 교정하고자 하는 회전 행렬은 도 2와 관련하여 설명한 바와 같이 리 군의 원소이나, 이러한 리 군의 원소는 벡터가 아니며, 예측된 회전 행렬의 예측 오차를 나타내는 회전 에러 행렬 또한 리 군의 원소로서 벡터가 아니다.The Kalman filter algorithm is a recursive algorithm that corrects a predetermined predicted physical state according to a predetermined measurement, and repeats operations for predicting a physical state based on the corrected state again. As a vector, it is preferable to use a vector. When data having a different structure from vectors, such as unit numbers, is used, various problems may arise, such as the need to continuously renormalize the state of the Kalman filter algorithm while executing the Kalman filter algorithm. In this regard, the rotation matrix to be corrected in the present embodiment is an element of the Li group as described with reference to FIG. 2, but the elements of the Li group are not vectors, and a rotation error matrix indicating a prediction error of the predicted rotation matrix. Also, it is not a vector as an element of Li group.

상술한 실시예에 있어서, 회전 행렬 생성부(120)는 회전 에러 행렬을 칼만 필터 알고리즘의 상태로 놓는 대신에, 칼만 필터 알고리즘에 적합한 구조를 갖는 벡터 값(즉, 회전 에러 행렬의 리 대수 값을 매개화한 벡터 값)을 칼만 필터 알고리즘의 상태로 놓고 교정 동작을 수행하기 때문에, 보다 효율적으로 회전 행렬의 교정 동작을 수행할 수 있다.In the above-described embodiment, instead of placing the rotation error matrix in the state of the Kalman filter algorithm, the rotation matrix generator 120 substitutes a vector value having a structure suitable for the Kalman filter algorithm (that is, the logarithmic value of the rotation error matrix). Since the calibrated operation is performed with the parameterized vector value in the state of the Kalman filter algorithm, the calibration operation of the rotation matrix can be performed more efficiently.

도 3은 도 1에 도시된 회전 행렬 생성부의 상세 블록도이다. 도 3을 참조하면, 회전 행렬 생성부(120)는 회전 행렬 초기화부(310), 바이어스 설정부(320), 회전 행렬 예측부(330), 회전 추정 행렬 생성부(340), 회전 에러 행렬 생성부(350) 및 회전 행렬 교정부(360)를 포함할 수 있다.3 is a detailed block diagram of the rotation matrix generator illustrated in FIG. 1. Referring to FIG. 3, the rotation matrix generator 120 may include a rotation matrix initialization unit 310, a bias setting unit 320, a rotation matrix prediction unit 330, a rotation estimation matrix generator 340, and a rotation error matrix generation. The unit 350 and the rotation matrix corrector 360 may be included.

회전 행렬 초기화부(310)는 회전 행렬 생성부(120)가 회전행렬을 예측 및 교정하는데 필요한 각종 변수들을 초기화할 수 있다. 일 실시예에 있어서, 회전 행렬 초기화부(310)는 회전 행렬 교정부(360)가 회전 에러 행렬을 교정하는데 필요한 칼만 필터 알고리즘의 프로세스 에러 공분산 값 및 측정 에러 공분산 값을 주어진 초기값으로 초기화할 수 있다. The rotation matrix initialization unit 310 may initialize various variables necessary for the rotation matrix generation unit 120 to predict and correct the rotation matrix. In one embodiment, the rotation matrix initialization unit 310 may initialize the process error covariance value and the measurement error covariance value of the Kalman filter algorithm necessary for the rotation matrix corrector 360 to correct the rotation error matrix to a given initial value. have.

바이어스 설정부(320)는 자이로 센서(111), 가속도 센서(112) 및 자기장 센서(113)가 제공하는 측정값들에 기초하여 자이로 센서(111)의 바이어스 값을 생성한다. 일 실시예에 있어서, 바이어스 설정부(320)는 위 측정값들에 기초하여 위 측정값들이 물체가 정지상태에서 측정된 값들인지 여부를 판단하고, 정지상태에서 측정된 값들인 경우, 자이로 센서(111)에서 제공한 측정값을 바이어스 값으로 설정할 수 있다. 바이어스 설정부(320)는 또한 새로이 제공된 측정값들이 정지상태에서 측정된 값들로 판단되는 경우에는, 새로이 제공된 측정값들 중 자이로 센서(111)에서 제공한 측정값을 바이어스 값으로 갱신할 수 있다.The bias setting unit 320 generates a bias value of the gyro sensor 111 based on the measured values provided by the gyro sensor 111, the acceleration sensor 112, and the magnetic field sensor 113. In an exemplary embodiment, the bias setting unit 320 determines whether the measured values are measured values in the stationary state based on the measured values, and if the measured values are measured in the stationary state, the gyro sensor ( The measured value provided in 111 can be set as the bias value. The bias setting unit 320 may also update the measured value provided by the gyro sensor 111 among the newly provided measured values to the bias value when it is determined that the newly provided measured values are measured in the stationary state.

회전 행렬 예측부(330)는 자이로 센서(111)에서 제공되는 측정값에서 상기 설정된 바이어스 값을 차감하여 각속도 값을 생성하고, 생성된 각속도 값으로부터 각속도 교대행렬을 생성한다. 회전 행렬 예측부(330)는 생성된 각속도 교대행렬에 기초하여 향후 회전 행렬을 예측할 수 있다.The rotation matrix predictor 330 generates an angular velocity value by subtracting the set bias value from the measured value provided by the gyro sensor 111, and generates an angular velocity shift matrix from the generated angular velocity value. The rotation matrix predictor 330 may predict a future rotation matrix based on the generated angular velocity alternating matrix.

회전 추정 행렬 생성부(340)는 회전 행렬 예측부(330)가 회전 행렬을 예측한 이후에 가속도 센서(112) 및 자기장 센서(113)로부터 제공되는 측정값들에 기초하여 현재 회전 행렬을 추정하는 회전 추정 행렬을 생성할 수 있다. 일 실시예에 있어서, 회전 추정 행렬 생성부(340)는 TRIAD 알고리즘을 사용하여 회전 추정 행렬을 생성할 수 있다. TRIAD 알고리즘 등 회전 추정 행렬을 생성하는 알고리즘은 당업계에 널리 알려져 있는바 자세한 설명은 생략한다.The rotation estimation matrix generator 340 estimates the current rotation matrix based on the measured values provided from the acceleration sensor 112 and the magnetic field sensor 113 after the rotation matrix predictor 330 predicts the rotation matrix. A rotation estimation matrix can be generated. In one embodiment, the rotation estimation matrix generator 340 may generate a rotation estimation matrix using a TRIAD algorithm. Algorithms for generating rotation estimation matrices, such as the TRIAD algorithm, are well known in the art, and thus detailed descriptions thereof will be omitted.

회전 에러 행렬 생성부(350)는 회전 행렬 예측부(330)에 의해 예측된 회전 행렬과 회전 추정 행렬 생성부(340)에 의해 생성된 회전 추정 행렬에 기초하여 예 측된 회전 행렬과 회전 추정 행렬 간의 차를 나타내는 회전 에러 행렬을 생성할 수 있다. 회전 에러 행렬 생성부(350)는 생성된 회전 에러 행렬의 리 대수 값을 구하고, 상기 리 대수 값을 매개화하는 벡터 값을 생성할 수 있다. 일 실시예에 있어서, 회전 에러 행렬 생성부(350)는 회전 에러 행렬에 지수 사상을 수행하여 회전 에러 행렬의 리 대수 값을 생성할 수 있다. 위 실시예에 있어서, 회전 에러 행렬 생성부(350)는 회전 에러 행렬의 로그 값을 상기 회전 에러 행렬의 리 대수로서 생성할 수 있다. The rotation error matrix generator 350 performs a prediction between the rotation matrix predicted by the rotation matrix predictor 330 and the rotation estimation matrix based on the rotation estimation matrix generated by the rotation estimation matrix generator 340. We can create a rotation error matrix that represents the difference. The rotation error matrix generator 350 may obtain the logical logarithm value of the generated rotation error matrix and generate a vector value that mediates the logical logarithm value. In one embodiment, the rotation error matrix generator 350 may generate the logarithmic value of the rotation error matrix by performing an exponential mapping on the rotation error matrix. In the above embodiment, the rotation error matrix generator 350 may generate a log value of the rotation error matrix as the logarithm of the rotation error matrix.

회전 행렬 교정부(360)는 회전 에러 행렬 생성부(350)에 의해 생성된 벡터 값을 칼만 필터 알고리즘의 상태로 설정하고, 위 상태에 칼만 필터 알고리즘을 수행하여 회전 에러 행렬의 리 대수 값을 교정할 수 있다. 회전 행렬 교정부(360)는 교정된 회전 에러 행렬의 리 대수 값에 기초하여 회전 에러 행렬을 교정하고, 교정된 회전 에러 행렬에 기초하여 상기 예측된 회전 행렬을 교정할 수 있다.  The rotation matrix corrector 360 sets the vector value generated by the rotation error matrix generator 350 to the state of the Kalman filter algorithm, and corrects the logarithmic value of the rotation error matrix by performing the Kalman filter algorithm to the above state. can do. The rotation matrix corrector 360 may correct the rotation error matrix based on the logarithmic value of the corrected rotation error matrix, and correct the predicted rotation matrix based on the corrected rotation error matrix.

이하, 도 3에 도시된 회전 행렬 생성부(120)의 각 구성요소들이 수행하는 동작들의 구체적인 내용에 대하여 수식들을 사용하여 자세히 설명한다. Hereinafter, specific details of operations performed by each component of the rotation matrix generator 120 illustrated in FIG. 3 will be described in detail using equations.

정의 및 표기법Definition and notation

항법 좌표계는 {N}으로 표기되고, 물체에 고정된 자세 추정 장치의 물체 좌표계는 {B}로 표기된다. 회전 행렬 R은 RN B로서 {B}의 {N}에 대한 상대적인 자세를 나타내고,

Figure 112009073939600-pat00004
는 R의 예측치 및 교정치를 나타낸다. wb는 {B}를 기준으로 측정된 각속도를 나타낸다.The navigation coordinate system is denoted by {N}, and the object coordinate system of the attitude estimation apparatus fixed to the object is denoted by {B}. Rotation matrix R is R N B , representing the relative attitude of {B} with respect to {N},
Figure 112009073939600-pat00004
Represents the predicted and corrected values of R. w b represents the angular velocity measured based on {B}.

회전 행렬의 지수 사상Exponential Mapping of Rotation Matrix

G가 일반 행렬 리 군을 나타내고, g가 그것의 리 대수를 나타낼 때, 지수 사상 exp:g->G는 지수 행렬(matrix exponential)로 정의된다. so(3)의 원소의 매개화된 벡터

Figure 112009073939600-pat00005
로 표현될 때, Rodrigue의 공식에 따르면 so(3)의 원소 [s]는 아래와 같은 공식으로 나타낼 수 있다.When G represents a general matrix li group and g represents its logarithm, the exponential event exp: g-> G is defined as a matrix exponential. a mediated vector of elements of so (3)
Figure 112009073939600-pat00005
According to Rodrigue's formula, the element [s] of so (3) can be expressed as

Figure 112009073939600-pat00006
Figure 112009073939600-pat00006

위에서 α 및 β는 θ가 0 근방인 값인 경우에도 잘 정의(well-defined)되어 있으며, 테일러 급수 확장을 사용하여 그 값들을 정확하게 구할 수 있다. 주어진 회전 행렬 R(즉, SO(3))마다 유일한 [s](즉, so(3))가 존재한다. 위 [s]의 크기는 구간 [0, π)내에 있으며, exp([s])=R의 관계가 성립한다. In the above, α and β are well-defined even when θ is near 0, and these values can be accurately obtained using Taylor series expansion. There is a unique [s] (ie so (3)) for each given rotation matrix R (i.e., SO (3)). The magnitude of the above [s] is in the interval [0, π), and the relationship of exp ([s]) = R holds.

θ가 π와 다른 경우에는 아래와 같은 식이 성립한다.If θ is different from π, the following equation holds.

Figure 112009073939600-pat00007
Figure 112009073939600-pat00007

여기서, rij는 회전 행렬의 (i,j)번째 요소를 나타내고, rij=[R]ij로도 표시된다. Here, r ij represents the (i, j) th element of the rotation matrix and is also represented by r ij = [R] ij .

θ가 π에 근접한 값인 경우에는 s가 다른 방식으로 계산되어야 한다. θ가 π에 근접한 값인 경우에는 테일러 급수를 이용하여 아래와 같은 식으로 나타낼 수 있다.If θ is close to π, s must be calculated in a different way. If θ is close to π, it can be expressed as below using Taylor series.

Figure 112009073939600-pat00008
Figure 112009073939600-pat00008

SO(3)를 사용한 자세 추정 방법Posture Estimation Method Using SO (3)

1. 센서 모델링1. Sensor Modeling

물체의 자세는 그것의 물체 좌표계 {B}와 항법 좌표계 {N}간의 관계를 특정하여 구할 수 있다. 가속도 센서 및 자기장 센서는 긴 주기를 갖는 측정값들을 제공하고, 자이로 센서는 짧은 주기를 갖는 측정값들을 제공한다. 자이로 센서가 측정한 각속도 값들은 회전 행렬을 예측하기 위한 프로세스 모델의 입력으로 사용된다. 가속도 센서 및 자기장 센서의 측정값들은 예측된 회전 행렬을 교정하는데 사용된다.The pose of an object can be obtained by specifying the relationship between its object coordinate system {B} and the navigation coordinate system {N}. Accelerometers and magnetic field sensors provide measurements with long periods, and gyro sensors provide measurements with short periods. The angular velocity values measured by the gyro sensor are used as inputs to the process model to predict the rotation matrix. The measurements of the acceleration sensor and the magnetic field sensor are used to calibrate the predicted rotation matrix.

물체는 다음과 같은 운동 방정식에 따라 그 자세가 변화한다.An object's posture changes according to the following equation of motion:

Figure 112009073939600-pat00009
Figure 112009073939600-pat00009

자이로 센서의 천천히 변화하는 바이어스 벡터 b는 백색 가우시안 잡음 벡터 ε에 의해 구동되는 1차 마르코프 프로세스의 실현(realization)으로 모사된다. The slowly changing bias vector b of the gyro sensor is simulated with the realization of the first order Markov process driven by the white Gaussian noise vector ε.

Figure 112009073939600-pat00010
Figure 112009073939600-pat00010

일 실시예에 있어서, 칼만 필터의 상태는 회전 에러 행렬의 리 대수 값을 매개화하는 벡터 값을 포함할 수 있다. 일 실시예에 있어서, 칼만 필터의 상태는 다음과 같은 벡터로 기술될 수 있다. In one embodiment, the state of the Kalman filter may comprise a vector value that mediates the logarithmic value of the rotation error matrix. In one embodiment, the state of the Kalman filter can be described by the following vector.

Figure 112009073939600-pat00011
Figure 112009073939600-pat00011

2. 프로세스 모델 - 회전 행렬의 예측2. Process Model-Prediction of Rotation Matrix

본 알고리즘의 프로세스 모델은 다음과 같이 기술될 수 있다.The process model of this algorithm can be described as follows.

Figure 112009073939600-pat00012
Figure 112009073939600-pat00012

여기서, R-는 예측된 회전행렬, R+는 이전에 교정된 회전 행렬 또는 초기화된 회전 행렬, h는 샘플링 시간을 나타낸다.Where R − is the predicted rotation matrix, R + is the previously corrected or initialized rotation matrix, and h is the sampling time.

칼만필터의 에러 상태는

Figure 112009073939600-pat00013
와 같이 계산될 수 있다.
Figure 112009073939600-pat00014
는 추정된 회전행렬과 측정된 행렬 간의 회전에러행렬을 나타낸다. 에러 상태의 상태 천이는 다음과 같은 공식에 의해 계산될 수 있다.The error condition of the Kalman Filter is
Figure 112009073939600-pat00013
It can be calculated as
Figure 112009073939600-pat00014
Denotes a rotation error matrix between the estimated rotation matrix and the measured matrix. The state transition of the error state can be calculated by the following formula.

Figure 112009073939600-pat00015
Figure 112009073939600-pat00015

여기서 vk는 백색 가우시안 잡음을 나타낸다. 에러 상태의 추정값과 에러 공분산 P의 추정값은 다음과 같은 공식에 의해 계산된다.Where v k represents white Gaussian noise. The estimated value of the error condition and the estimated value of the error covariance P are calculated by the following formula.

Figure 112009073939600-pat00016
Figure 112009073939600-pat00016

Figure 112009073939600-pat00017
Figure 112009073939600-pat00017

3. 측정 모델3. Measurement Model

자세를 연산하는데 필요한 최소 측정 벡터 개수는 두 개이다. TRIAD 알고리 즘은 Wahba의 문제를 해결하기 위한 단일 좌표계의 확정적 방법이다. TRIAD 알고리즘을 사용하면 가속도 센서 및 자기장 센서의 정규화된 측정값들로부터 회전추정행렬 Yk +1을 구할 수 있다. 회전추정행렬 Yk +1은 {B}의 {N}에 대한 상대적인 자세를 나타낸다. Yk+1 및 이전 예측된 회전행렬 R-에 기초해서

Figure 112009073939600-pat00018
의 측정값 y를 다음과 같이 구할 수 있다.The minimum number of measurement vectors needed to calculate the pose is two. The TRIAD algorithm is a definitive method of single coordinate system to solve Wahba's problem. Using the TRIAD algorithm, the rotation estimation matrix Y k +1 can be obtained from the normalized measurements of the acceleration and magnetic field sensors. The rotation estimation matrix Y k +1 represents a pose of {B} relative to {N}. Based on Y k + 1 and the previously predicted rotation matrix R-
Figure 112009073939600-pat00018
The measured value of y can be found as

Figure 112009073939600-pat00019
Figure 112009073939600-pat00019

Figure 112009073939600-pat00020
Figure 112009073939600-pat00020

에러 상태의 칼만 이득 K는 다음과 같이 계산된다.The Kalman gain K of the error state is calculated as follows.

Figure 112009073939600-pat00021
Figure 112009073939600-pat00021

Figure 112009073939600-pat00022
Figure 112009073939600-pat00022

Figure 112009073939600-pat00023
Figure 112009073939600-pat00023

위 공식 중 칼만이득 Kk +1에 대한 공식 중 Rk +1은 회전 행렬이 아니라 측정 잡음의 공분산을 나타낸다.Of the above formulas for Kalmandeuk K k +1 , R k +1 represents the covariance of the measured noise, not the rotation matrix.

추정된 SO(3) 행렬은 so(3)인

Figure 112009073939600-pat00024
의 지수와 곱해져서 갱신된다.The estimated SO (3) matrix is so (3)
Figure 112009073939600-pat00024
It is updated by multiplying by the exponent of.

Figure 112009073939600-pat00025
Figure 112009073939600-pat00025

중력 및 자기장 벡터의 측정값들은 {B}에 대한 고정 관성 방향에서의 측정값 들이다. The measurements of gravity and magnetic field vectors are in the direction of fixed inertia with respect to {B}.

도 4는 본 발명의 일 실시예에 따른 자세 추정 방법의 절차도이다. 도 4를 참조하면, 자세 추정 장치의 자세 회전 행렬 초기화부는 자세 추정 장치가 회전행렬을 예측 및 교정하는데 필요한 각종 변수들을 초기화할 수 있다(단계 410). 단계 420에서, 자세 추정 장치의 바이어스 설정부는 자세 추정 장치의 관성 측정부(예를 들어, 자이로 센서, 가속도 센서 및 자기장 센서)가 제공하는 측정값들에 기초하여 자이로 센서의 바이어스 값을 생성한다. 단계 430에서, 자세 추정 장치의 회전 행렬 예측부는 자이로 센서에서 제공되는 측정값에서 상기 설정된 바이어스 값을 차감하여 각속도 값을 생성하고, 생성된 각속도 값으로부터 향후 회전 행렬을 예측한다. 단계 440에서, 자세 추정 장치의 회전 추정 행렬 생성부는 회전 행렬 예측부가 회전 행렬을 예측한 이후에 가속도 센서 및 자기장 센서로부터 제공되는 측정값들에 기초하여 현재 회전 행렬을 추정하는 회전 추정 행렬을 생성한다. 단계 450에서, 자세 추정 장치의 회전 에러 행렬 생성부는 회전 행렬 예측부에 의해 예측된 회전 행렬과 회전 추정 행렬 생성부에 의해 생성된 회전 추정 행렬에 기초하여 예측된 회전 행렬과 회전 추정 행렬 간의 차를 나타내는 회전 에러 행렬을 생성한다. 단계 460에서, 자세 추정 장치의 회전 행렬 교정부는 회전 에러 행렬 생성부에 의해 생성된 벡터 값을 칼만 필터 알고리즘의 상태로 설정하고, 위 상태에 칼만 필터 알고리즘을 수행하여 회전 에러 행렬의 리 대수 값을 교정하고, 회전 행렬 교정부는 교정된 회전 에러 행렬에 기초하여 상기 예측된 회전 행렬을 교정한다. 단계 470에서, 자세 추정 장치는 관성 측정부에서 추가 측정값들이 제공되는 지 여부를 판단하고, 제공되는 경우 단계 420으로 이동하고, 제공되지 않는 경우 절차를 종료한다.4 is a flowchart illustrating a pose estimation method according to an embodiment of the present invention. Referring to FIG. 4, the attitude rotation matrix initialization unit of the attitude estimation apparatus may initialize various variables necessary for the attitude estimation apparatus to predict and correct the rotation matrix (step 410). In operation 420, the bias setting unit of the attitude estimation apparatus generates a bias value of the gyro sensor based on the measured values provided by the inertial measurement unit (eg, a gyro sensor, an acceleration sensor, and a magnetic field sensor) of the attitude estimation apparatus. In operation 430, the rotation matrix predictor of the attitude estimation apparatus generates an angular velocity value by subtracting the set bias value from the measured value provided by the gyro sensor, and predicts a future rotation matrix from the generated angular velocity value. In operation 440, the rotation estimation matrix generator of the posture estimating apparatus generates a rotation estimation matrix that estimates the current rotation matrix based on measurements provided from the acceleration sensor and the magnetic field sensor after the rotation matrix predictor predicts the rotation matrix. . In operation 450, the rotation error matrix generator of the attitude estimation apparatus calculates a difference between the rotation matrix predicted by the rotation matrix predictor and the rotation estimation matrix based on the rotation estimation matrix generated by the rotation estimation matrix generator. Create a rotation error matrix that represents In step 460, the rotation matrix corrector of the attitude estimation apparatus sets the vector value generated by the rotation error matrix generator to the state of the Kalman filter algorithm, and performs the Kalman filter algorithm on the above state to set the logical logarithm value of the rotation error matrix. And the rotation matrix corrector corrects the predicted rotation matrix based on the corrected rotation error matrix. In operation 470, the attitude estimating apparatus determines whether additional measurements are provided by the inertial measurement unit, and if so, moves to operation 420, and if not, ends the procedure.

도 5는 본 발명의 일 실시예에 따른 바이어스 값 설정 및 갱신 방법의 절차도이다. 도 5를 참조하면, 자세 추정 장치의 바이어스 설정부는 자세 추정 장치의 가속도 센서, 자기장 센서 및 자이로 센서로부터 각각 측정값을 입력받는다(단계 510). 단계 520에서, 바이어스 설정부는 가속도 센서의 측정값이 지구 중력 값을 포함하는 제1 범위 내에 있는지 여부를 판단한다. 제1 범위 내에 있는 경우, 단계 530에서, 바이어스 설정부는 가속도 센서의 측정값과 상기 자기장 센서의 측정값 간의 내적 값이 제2 범위 내에 있는지 여부를 판단한다. 제2 범위 내에 있는 경우, 단계 540에서, 바이어스 설정부는 소정의 시간에 걸쳐 상기 자이로 센서에서 제공한 측정값들이 모두 제3 범위 내에 있는지 여부를 판단한다. 모두 제3 범위 내에 있는 경우, 바이어스 설정부는 자이로 센서의 측정값을 바이어스 값으로 설정 또는 갱신하고, 절차를 종료한다. 반면에, 제1 범위 내에 있지 않거나, 제2 범위 내에 있지 않거나, 제3 범위 내에 있지 않는 경우, 절차를 종료한다. 5 is a flowchart illustrating a bias value setting and updating method according to an embodiment of the present invention. Referring to FIG. 5, the bias setting unit of the attitude estimation apparatus receives measurement values from the acceleration sensor, the magnetic field sensor, and the gyro sensor of the attitude estimation apparatus, respectively (step 510). In step 520, the bias setting unit determines whether the measured value of the acceleration sensor is within a first range including the earth gravity value. If within the first range, in step 530, the bias setting unit determines whether the internal value between the measured value of the acceleration sensor and the measured value of the magnetic field sensor is within the second range. If within the second range, in step 540, the bias setting unit determines whether all the measurement values provided by the gyro sensor are within the third range over a predetermined time. If all are within the third range, the bias setting unit sets or updates the measured value of the gyro sensor to the bias value and ends the procedure. On the other hand, if it is not within the first range, not within the second range, or not within the third range, the procedure ends.

당업자는 본 명세서에 상술된 및 다른 프로세스들, 상호작용들 및 방법들에 있어서, 프로세스들, 상호 작용들 및 방법들 내에서 수행되는 기능들은 상이한 순서로 구현될 수 있다는 것을 알 것이다. 이에 더하여, 설명된 단계들 및 동작들은 예시로만 제공된다. 즉, 단계들 및 동작들 중 일부는 선택적일 수 있으며, 개시된 실시예들의 본질을 훼손하지 않으면서, 보다 적은 단계들 및 동작들로 합쳐지거나, 추가적인 단계들 및 동작들로 확장될 수 있다. Those skilled in the art will appreciate that in the processes described above and other processes, interactions, and methods, the functions performed within the processes, interactions, and methods may be implemented in a different order. In addition, the steps and operations described are provided by way of example only. That is, some of the steps and actions may be optional and may be combined into fewer steps and actions or extended to additional steps and actions without compromising the nature of the disclosed embodiments.

당업자는 또한 본 명세서에 설명된 장치들 및 방법들은 하드웨어, 소프트웨어, 펌웨어, 미들웨어 또는 이들의 조합들로 구현될 수 있고, 시스템들, 서브시스템들, 구성품들 또는 그들의 서브 구성품들에 사용될 수 있다는 것을 알 것이다. 예를 들어, 소프트웨어로 구현된 방법은 방법의 동작들을 수행하기 위한 컴퓨터 코드를 포함할 수 있다. 이러한 컴퓨터 코드는 프로세서 판독 가능 매체 또는 컴퓨터 프로그램 제품과 같은 기계로 판독 가능한 매체에 저장되거나, 반송파, 또는 반송파에 의해 변조된 신호로 구현되는 컴퓨터 데이터 신호로서 전송 매체 또는 통신 링크를 통해 전송될 수 있다. 기계로 판독 가능한 매체 또는 프로세서로 판독 가능한 매체는 기계(예를 들어, 프로세서, 컴퓨터 등)에 의해 판독되고 실행될 수 있는 형태로 정보를 저장 또는 전달할 수 있는 임의의 매체를 포함할 수 있다.Those skilled in the art will also appreciate that the devices and methods described herein may be implemented in hardware, software, firmware, middleware, or combinations thereof, and may be used in systems, subsystems, components, or subcomponents thereof. Will know. For example, a method implemented in software can include computer code for performing the operations of the method. Such computer code may be stored on a machine readable medium, such as a processor readable medium or a computer program product, or transmitted over a transmission medium or communication link as a computer data signal embodied as a carrier wave or a signal modulated by a carrier wave. . Machine-readable media or processor-readable media can include any medium that can store or convey information in a form that can be read and executed by a machine (eg, processor, computer, etc.).

상술된 사항으로부터, 본 명세서의 다양한 실시예들은 예시를 목적으로 설명되었으며, 본 명세서의 범위 및 취지를 벗어나지 않으면서 다양한 변형된 형태로 구현할 수 있다. 따라서, 본 명세서에 기술된 다양한 실시예들은 제한적인 의미를 가지지 않으며, 본 명세서의 진정한 범위 및 취지는 후술되는 특허청구범위에 의해 나타내어진다.From the foregoing, various embodiments of the present disclosure have been described for purposes of illustration, and may be implemented in various modified forms without departing from the scope and spirit of the present disclosure. Accordingly, the various embodiments described herein are not to be taken in a limiting sense, and the true scope and spirit of the specification are indicated by the following claims.

도 1은 본 발명의 일 실시예에 따른 자세 추정 장치의 블록도.1 is a block diagram of an attitude estimation apparatus according to an embodiment of the present invention.

도 2는 본 발명의 일 실시예에 따른 물체의 회전 행렬을 설명하기 위한 개념도.2 is a conceptual diagram illustrating a rotation matrix of an object according to an embodiment of the present invention.

도 3은 도 1에 도시된 회전 행렬 생성부의 상세 블록도.FIG. 3 is a detailed block diagram of the rotation matrix generator shown in FIG. 1. FIG.

도 4는 본 발명의 일 실시예에 따른 자세 추정 방법의 절차도.4 is a flowchart of a pose estimation method according to an embodiment of the present invention.

도 5는 본 발명의 일 실시예에 따른 바이어스 값 설정 및 갱신 방법의 절차도.5 is a flowchart illustrating a bias value setting and updating method according to an embodiment of the present invention.

Claims (16)

물체에 부착된 관성 측정 장치로부터 제공되는 상기 물체에 대한 각속도 값, 상기 물체가 받는 중력 값 및 상기 물체에서의 자기장 값에 기초하여 컴퓨팅 장치에서 상기 물체의 자세를 추정하는 방법으로서,A method of estimating an attitude of an object in a computing device based on an angular velocity value, an gravity value received by the object, and a magnetic field value of the object provided from an inertial measurement device attached to an object, 상기 각속도 값에 기초하여 상기 물체의 회전 행렬 - 상기 회전 행렬은 상기 물체의 자세를 나타냄 - 을 예측(predict)하는 단계;Predicting a rotation matrix of the object, the rotation matrix representing a pose of the object based on the angular velocity value; 상기 중력 값 및 상기 자기장 값으로부터 회전 에러 행렬 - 상기 회전 에러 행렬은 상기 예측된 회전 행렬의 예측 오차를 나타냄 - 을 생성하는 단계;Generating a rotation error matrix from the gravity value and the magnetic field value, the rotation error matrix representing a prediction error of the predicted rotation matrix; 상기 회전 에러 행렬의 리 대수(Lie algebra) 값을 생성하는 단계; 및Generating Li algebra values of the rotation error matrix; And 상기 회전 에러 행렬의 리 대수 값에 기초하여 칼만 필터 알고리즘에 따라 상기 예측된 회전 행렬을 교정(correct)하는 단계Correcting the predicted rotation matrix according to a Kalman filter algorithm based on the logical logarithm value of the rotation error matrix 를 포함하는 방법.How to include. 제1항에 있어서, 상기 예측된 회전 행렬을 교정하는 단계는, The method of claim 1, wherein correcting the predicted rotation matrix comprises: 상기 회전 에러 행렬의 리 대수 값을 매개화하는 벡터 값을 생성하는 단계;Generating a vector value that mediates the logical logarithm of the rotation error matrix; 상기 벡터 값을 상기 칼만 필터 알고리즘의 상태(state)로 제공하는 단계; Providing the vector value as a state of the Kalman filter algorithm; 상기 제공된 상태에 기초하여 상기 칼만 필터 알고리즘을 수행하여 상기 벡터 값을 교정하는 단계; 및 Correcting the vector value by performing the Kalman filter algorithm based on the provided state; And 상기 교정된 벡터 값에 기초하여 상기 예측된 회전 행렬을 교정하는 단계Calibrating the predicted rotation matrix based on the calibrated vector value 를 포함하는 방법.How to include. 제1항에 있어서, 상기 회전 에러 행렬의 리 대수 값을 생성하는 단계는, The method of claim 1, wherein generating the logarithmic value of the rotation error matrix comprises: 상기 회전 에러 행렬에 지수 사상(exponential mapping)을 수행하여 상기 회전 에러 행렬의 리 대수를 생성하는 단계Generating logical numbers of the rotation error matrix by performing exponential mapping on the rotation error matrix 를 포함하는, 방법./ RTI > 제3항에 있어서, 상기 회전 에러 행렬의 리 대수를 생성하는 단계는, The method of claim 3, wherein generating the logarithm of the rotation error matrix, 상기 회전 에러 행렬의 로그 값을 상기 회전 에러 행렬의 리 대수로서 생성하는 단계Generating the logarithm of the rotation error matrix as the logarithm of the rotation error matrix 를 포함하는, 방법./ RTI > 제1항에 있어서, 상기 회전 에러 행렬을 생성하는 단계는, The method of claim 1, wherein generating the rotation error matrix, 상기 중력 값 및 상기 자기장 값에 기초하여 상기 회전 추정 행렬을 생성하는 단계; Generating the rotation estimation matrix based on the gravity value and the magnetic field value; 상기 예측된 회전 행렬과 상기 회전 추정 행렬에 기초하여 상기 회전 에러 행렬을 생성하는 단계 - 상기 회전 에러 행렬은 상기 예측된 회전 행렬과 상기 회전 추정 행렬 간의 차를 나타냄 - Generating the rotation error matrix based on the predicted rotation matrix and the rotation estimation matrix, wherein the rotation error matrix indicates a difference between the predicted rotation matrix and the rotation estimation matrix. 를 포함하는 방법.How to include. 제1항에 있어서, 상기 물체의 회전 행렬을 예측하는 단계는, The method of claim 1, wherein predicting the rotation matrix of the object comprises: 자이로 센서의 바이어스 값을 설정하는 단계; Setting a bias value of the gyro sensor; 상기 자이로 센서로부터 측정값을 제공받는 단계; 및 Receiving a measurement value from the gyro sensor; And 상기 측정값에서 상기 바이어스 값을 차감한 값을 상기 각속도 값으로서 생성하는 단계Generating a value obtained by subtracting the bias value from the measured value as the angular velocity value 를 포함하는 방법.How to include. 제6항에 있어서, 상기 자이로 센서의 바이어스 값을 설정하는 단계는, The method of claim 6, wherein the setting of the bias value of the gyro sensor comprises: 가속도 센서, 자기장 센서 및 상기 자이로 센서로부터 각각 측정값을 제공받는 단계; Receiving measurement values from an acceleration sensor, a magnetic field sensor, and the gyro sensor, respectively; 상기 가속도 센서의 측정값이 지구 중력 값을 포함하는 제1 범위 내에 있는지 여부를 판단하는 단계; Determining whether the measured value of the acceleration sensor is within a first range including an earth gravity value; 상기 가속도 센서의 측정값과 상기 자기장 센서의 측정값 간의 내적 값이 제2 범위 내에 있는지 여부를 판단하는 단계; 및 Determining whether an internal value between the measured value of the acceleration sensor and the measured value of the magnetic field sensor is within a second range; And 상기 가속도 센서의 측정값이 상기 제1 구간 내에 있고, 상기 내적 값이 상기 제2 구간 내에 있는 경우, 상기 자이로 센서의 측정값을 바이어스 값으로 설정하는 단계Setting the measured value of the gyro sensor as a bias value when the measured value of the acceleration sensor is within the first section and the inner product is within the second section. 를 포함하는 방법.How to include. 제7항에 있어서, 상기 자이로 센서의 바이어스 값을 설정하는 단계는, The method of claim 7, wherein the setting of the bias value of the gyro sensor, 소정의 시간에 걸쳐 상기 자이로 센서에서 제공한 측정값들이 모두 제3 범위 내에 있는지 여부를 판단하는 단계Determining whether all of the measured values provided by the gyro sensor are within a third range over a predetermined time; 를 더 포함하는 방법.How to include more. 물체의 자세를 추정하는 장치로서,An apparatus for estimating the pose of an object, 상기 물체에 부착되는 관성 측정부 - 상기 관성 측정부는 상기 물체에 대한 각속도 값, 상기 물체가 받는 중력 값 및 상기 물체에서의 자기장 값을 제공함 - ;An inertial measurement unit attached to the object, the inertial measurement unit providing an angular velocity value for the object, a gravity value received by the object, and a magnetic field value in the object; 상기 각속도 값에 기초하여 상기 물체의 회전 행렬 - 상기 회전 행렬은 상기 물체의 자세를 나타냄 - 을 예측(predict)하고, 상기 중력 값 및 상기 자기장 값으로부터 회전 에러 행렬 - 상기 회전 에러 행렬은 상기 예측된 회전 행렬의 예측 오차를 나타냄 -- 생성하고, 상기 회전 에러 행렬의 리 대수(Lie algebra) 값을 생성하고, 상기 회전 에러 행렬의 리 대수 값에 기초하여 칼만 필터 알고리즘에 따라 상기 예측된 회전 행렬을 교정(correct)하는 회전 행렬 생성부Predict a rotation matrix of the object based on the angular velocity value, the rotation matrix representing the pose of the object, a rotation error matrix from the gravity value and the magnetic field value, and the rotation error matrix Represent a prediction error of the rotation matrix, generate a Li algebra value of the rotation error matrix, and generate the predicted rotation matrix according to the Kalman filter algorithm based on the Li algebra values of the rotation error matrix. Correction Rotation Matrix Generator 를 포함하는 장치./ RTI > 제9항에 있어서, 상기 회전 행렬 생성부는 상기 회전 에러 행렬의 리 대수 값을 매개화하는 벡터를 상기 칼만 필터 알고리즘의 상태(state)로 제공하고, 상기 제공된 상태에 기초하여 상기 칼만 필터 알고리즘을 수행하여 상기 벡터를 교정하고, 상기 교정된 벡터에 기초하여 상기 예측된 회전 행렬을 교정하는, 장치.The method of claim 9, wherein the rotation matrix generator provides a vector for mediating the logarithmic value of the rotation error matrix as a state of the Kalman filter algorithm, and performs the Kalman filter algorithm based on the provided state. Calibrate the vector and calibrate the predicted rotation matrix based on the calibrated vector. 제9항에 있어서, 상기 회전 행렬 생성부는 상기 회전 에러 행렬에 지수 사상(exponential mapping)을 수행하여 상기 회전 에러 행렬의 리 대수를 생성하는, 장치.10. The apparatus of claim 9, wherein the rotation matrix generator generates logical numbers of the rotation error matrix by performing exponential mapping on the rotation error matrix. 제9항에 있어서, 상기 회전 행렬 생성부는 상기 회전 에러 행렬의 로그 값을 상기 회전 에러 행렬의 리 대수로서 생성하는, 장치.10. The apparatus of claim 9, wherein the rotation matrix generator generates a log value of the rotation error matrix as the logarithm of the rotation error matrix. 제9항에 있어서, 상기 회전 행렬 생성부는 상기 중력 값 및 상기 자기장 값에 기초하여 상기 회전 추정 행렬을 생성하고, 상기 예측된 회전 행렬과 상기 회전 추정 행렬에 기초하여 상기 회전 에러 행렬을 생성하고, 상기 회전 에러 행렬은 상기 예측된 회전 행렬과 상기 회전 추정 행렬 간의 차를 나타내는, 장치.The apparatus of claim 9, wherein the rotation matrix generator generates the rotation estimation matrix based on the gravity value and the magnetic field value, and generates the rotation error matrix based on the predicted rotation matrix and the rotation estimation matrix. And the rotation error matrix is indicative of the difference between the predicted rotation matrix and the rotation estimation matrix. 제9항에 있어서, 상기 회전 행렬 생성부는 자이로 센서의 바이어스 값을 설정하고, 상기 자이로 센서로부터 측정값을 제공받고, 상기 측정값에서 상기 바이어스 값을 차감한 값을 상기 각속도 값으로서 생성하고, 상기 각속도 값에 지수 사상을 수행하여 상기 각속도 값의 리 대수를 생성하는, 장치.The method of claim 9, wherein the rotation matrix generator sets a bias value of a gyro sensor, receives a measurement value from the gyro sensor, and generates a value obtained by subtracting the bias value from the measurement value as the angular velocity value. Performing exponential mapping on the angular velocity value to produce the logarithm of the angular velocity value. 제14항에 있어서, 상기 회전 행렬 생성부는 가속도 센서, 자기장 센서 및 상기 자이로 센서로부터 각각 측정값을 제공받고, 상기 가속도 센서의 측정값이 지구 중력 값을 포함하는 제1 범위 내에 있는지 여부를 판단하고, 상기 가속도 센서의 측정값과 상기 자기장 센서의 측정값 간의 내적 값이 제2 범위 내에 있는지 여부를 판단하고, 상기 가속도 센서의 측정값이 상기 제1 구간 내에 있고, 상기 내적 값이 상기 제2 구간 내에 있는 경우, 상기 자이로 센서의 측정값을 바이어스 값으로 설정하는, 장치.15. The method of claim 14, wherein the rotation matrix generator is provided with a measurement value from the acceleration sensor, the magnetic field sensor and the gyro sensor, respectively, and determines whether the measured value of the acceleration sensor is within the first range including the earth gravity value Determine whether an internal value between the measured value of the acceleration sensor and the measured value of the magnetic field sensor is within a second range, the measured value of the acceleration sensor is within the first section, and the internal value is the second section If within, set the measurement of the gyro sensor to a bias value. 삭제delete
KR1020090117306A 2009-11-20 2009-11-30 Method and apparatus for estimating orientation by using lie algebra and kalman filter KR101140379B1 (en)

Applications Claiming Priority (2)

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

Publications (2)

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

Family

ID=44364790

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020090117306A KR101140379B1 (en) 2009-11-20 2009-11-30 Method and apparatus for estimating orientation by using lie algebra and kalman filter

Country Status (1)

Country Link
KR (1) KR101140379B1 (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101833217B1 (en) 2011-12-07 2018-03-05 삼성전자주식회사 Mobile terminal device for positioning system based on magnetic map and positioning method using the device
KR101462007B1 (en) * 2013-08-22 2014-11-18 한국과학기술연구원 Apparatus for estimating attitude and method for estimating attitude
CN114459507B (en) * 2022-03-03 2024-02-09 湖南大学无锡智能控制研究院 DVL installation error calibration method, device and system

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009115545A (en) 2007-11-05 2009-05-28 Denso Corp Vehicle present position detection system and program

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009115545A (en) 2007-11-05 2009-05-28 Denso Corp Vehicle present position detection system and program

Also Published As

Publication number Publication date
KR20110056185A (en) 2011-05-26

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 (en) System and method for gyrocompass alignment using dynamically calibrated sensor data and iterative extended Kalman filter in a navigation system
US9417091B2 (en) System and method for determining and correcting field sensors errors
CN106500693B (en) A kind of AHRS algorithm based on adaptive extended kalman filtering
JP4876204B2 (en) Small attitude sensor
JP4199553B2 (en) Hybrid navigation device
JP2014089113A (en) Posture estimation device and program
KR20140025319A (en) Apparatuses and methods for dynamic tracking and compensation of magnetic near field
KR20060048581A (en) System for navigation redundancy
JP2012173190A (en) Positioning system and positioning method
CN108318027B (en) Method and device for determining attitude data of carrier
CN112798021B (en) Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter
Troni et al. Adaptive Estimation of Measurement Bias in Three-Dimensional Field Sensors with Angular Rate Sensors: Theory and Comparative Experimental Evaluation.
JP5164645B2 (en) Method and apparatus for repetitive calculation control in Kalman filter processing
EP2930467A1 (en) A system and method for sensing the inclination of a moving platform with respect to gravity
JP2015179002A (en) Attitude estimation method, attitude estimation device and program
JP2019120587A (en) Positioning system and positioning method
KR101140379B1 (en) Method and apparatus for estimating orientation by using lie algebra and kalman filter
CN110567481A (en) object displacement monitoring method
Lee et al. Interference-compensating magnetometer calibration with estimated measurement noise covariance for application to small-sized UAVs
JP2013122384A (en) Kalman filter and state estimation device
CN114964214B (en) Extended Kalman filtering attitude calculation method of attitude heading reference system
Munguia et al. An attitude and heading reference system (AHRS) based in a dual filter
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
JP2006038650A (en) Posture measuring method, posture controller, azimuth meter and computer program

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