KR101462007B1 - Apparatus for estimating attitude and method for estimating attitude - Google Patents

Abstract

The present invention relates to an attitude estimating apparatus and an attitude estimating method. According to an embodiment of the present invention, an attitude estimating apparatus comprises: a sensor unit which has a speed sensor, an earth magnetic field sensor, and a gyro sensor; an orientation calculation unit which calculates orientation using data measured by the sensor unit; a gyro integration attitude updating unit which updates a gyro integration attitude by performing integration from a compensation value provided by the gyro sensor; a prediction model execution unit which performs a calculation of a prediction model of the Kalman filter; a measurement model execution unit which performs a calculation of a measurement model of the Kalman filter; a sensor calibration unit which compensates the calibration values of the speed sensor, the earth magnetic field sensor, and the gyro sensor by using perturbation when the perturbation is detected; and a noise covariance correction unit which corrects process noise covariance from an estimated state variable.

Description

[0001] The present invention relates to an attitude estimation method and an attitude estimation method,

The present invention relates to an attitude estimation apparatus and an attitude estimation method.

2. Description of the Related Art In recent years, portable terminal devices such as mobile phones and tablets are often equipped with sensors for measuring changes in attitude or attitude of portable terminal devices such as acceleration sensors, geomagnetic sensors or gyro sensors. An inertial measurement unit (IMU) is known as an apparatus equipped with such attitude measuring sensors.

However, such an IMU has a problem that it is difficult to estimate an accurate posture due to a large error in a measurement value, and in some cases, a measurement value diverges and a posture measurement function is lost.

Recently, a Kalman filter is used for the posture estimation. In this case, the unit quaternion is used as a state variable of the Kalman filter to estimate the posture. However, the state variable of the Kalman filter must be applied only to the vector space, the number of unit employees is not closed by the addition operation, and the number of unit employees is set to 1 , There may arise a problem that the covariance matrix has a singularity due to the additional constraint condition that the covariance matrix should be satisfied.

In this case, since the sigma point in the measurement step is set to the gravitational or geomagnetic vector measured from the acceleration sensor or the geomagnetic sensor, There is a problem that the amount of computation of the non-directed Kalman filter increases greatly as the number of sigma points increases.

Also, the bias of the gyro sensor can be estimated using Kalman filter for the posture estimation. In this case, the error between the predicted posture and the measurement posture may be used, or a method of using the vector value of the gravity direction and the measurement value of the gyro sensor . This method follows the method of real-time prediction by setting the gyro bias as the state variable of the Kalman filter in most cases. However, when the gyro bias is set as the state variable of the Kalman filter, there is a disadvantage that the amount of calculation of the filter is increased and the degree of the state variable is increased. Due to this problem, the performance of the gyro sensor has recently been improved, so that the gyro bias is modeled in a form that assumes a value that changes very slowly depending on an unknown value or time.

In addition, the noise covariance of the Kalman filter used in the posture estimation is often set to a constant value. In recent years, a process noise or a measurement noise covariance considering a perturbation such as a motion with high acceleration or an object with a large magnetic field, In real time.

An embodiment of the present invention is to provide an attitude estimation apparatus and attitude estimation method having high accuracy and measurement stability.

An orientation estimation apparatus according to an embodiment of the present invention includes a sensor unit including an acceleration sensor, a geomagnetic sensor, and a gyro sensor; and an initial posture calculation unit that calculates an initial orientation using data measured by the sensor unit A gyro integration posture updating unit for updating the gyro integration posture by performing integration from the correction value of the gyro sensor; a prediction model performing unit for performing a calculation of a prediction model of the Kalman filter; A sensor calibration unit for correcting the calibration values of the acceleration sensor, the ground magnetic sensor and the gyro sensor by using the disturbance when perturbation is sensed, process noise covariance matrix.

The Kalman filter is a sigma point Kalman filter. The prediction model performing unit includes a sigma point calculating unit for calculating a sigma point at a prediction step of a sigma point Kalman filter, a sigma point decomposing unit for decomposing a sigma point at the prediction step, And a state variable and a covariance update unit for updating the state variable and the covariance from the sigma point.

The Kalman filter may be a sigma point Kalman filter. The measurement model performing unit may include a sigma point calculating unit that calculates a sigma point at a measurement step from a sigma point at a prediction step, a gyro integration posture, an acceleration sensor, An innovation calculation section for calculating an innovation of a Kalman filter using a point calculation section, a sigma point in a prediction step, and a sigma point in a measurement step, and an innovation calculation section for calculating a posterior state variable and a covariance And a posture attitude calculating unit for calculating a posture calculated from the exponent mapping of the state variable and a posture from the preprocessing posture.

The above-described posture estimating apparatus may further comprise a measurement model exchanging unit for changing the measurement model in accordance with the presence or absence of the disturbance.

The measurement model exchange unit may further include a motion disturbance determination unit that determines whether or not disturbance is caused by motion of the attitude estimation device and a magnetic field disturbance determination unit that determines whether or not the magnetic field is disturbed, A second pre-processing posture calculating unit for calculating a pre-processing posture from the measured value of the acceleration sensor and the measured value of the geomagnetic sensor, and a pre-processing posture calculating unit for calculating a preprocessing posture from the gyro integration posture And a third pre-processing attitude calculating unit for calculating the posture, and any one of the first to third pre-processing attitude calculating units may alternatively be operated depending on the presence or absence of motion disturbance and magnetic field disturbance.

According to another aspect of the present invention, there is provided a posture estimation method of an orientation estimation device including an acceleration sensor, a geomagnetic sensor and a gyro sensor, the method comprising the steps of: Performing a calculation of a predictive model of a Kalman filter, performing a calculation of a measurement model of a Kalman filter, calculating a correction value of the gyro sensor, And correcting the calibration values of the acceleration sensor, the geomagnetic sensor and the gyro sensor using the disturbance when the disturbance is detected, and correcting the process noise covariance from the estimated state variables.

Wherein the Kalman filter is a sigma point Kalman filter and performing the operation of the prediction model of the Kalman filter comprises calculating a sigma point at a prediction step of a sigma point Kalman filter, And updating the state variable and the covariance from the sigma point.

Wherein the Kalman filter is a sigma point Kalman filter and the step of performing the calculation of the Kalman filter measurement model comprises the steps of measuring from a pre-processing posture calculated from a sigma point, a gyro integration posture, an acceleration sensor, Calculating a sigma point at a prediction step, calculating an innovation of a Kalman filter using a sigma point at a prediction step and a sigma point at a measurement step, and using the innovation of the Kalman filter to calculate a posterior state Updating a variable and a covariance, calculating a posture calculated from the exponent mapping of the state variable and a posture posture from the preprocessing posture.

Further, the above-described posture estimation method may further include a step of changing the measurement model in accordance with the presence or absence of the disturbance.

The step of changing the measurement model may include the steps of determining whether or not disturbance is caused by motion of the attitude estimation apparatus, determining whether disturbance of the magnetic field is present, determining whether the disturbance of the magnetic field is present or not, Calculating a preprocessing posture from the measurement value of the sensor and the measurement value of the gyro sensor, calculating a preprocessing posture from the measurement value of the acceleration sensor and the measurement value of the geomagnetic sensor, or calculating a preprocessing posture from the gyro integration posture Or alternatively may include any of the following steps.

The step of changing the measurement model includes the steps of: determining whether disturbance is caused by motion of the attitude estimation apparatus; determining whether disturbance of the magnetic field is present; determining whether or not disturbance of the magnetic field is present; Calculating a preprocessing posture from the measurement values of the gyro sensor and the measured values of the gyro sensor, re-calibrating the geomagnetic sensor, calculating a pre-processing posture from the measured values of the acceleration sensor and the gyro sensor, And calculating the pre-processing posture from the posture.

According to the posture estimation apparatus and posture estimation method according to an embodiment of the present invention, it is possible to measure the posture with high accuracy and measurement stability.

Fig. 1 is a view schematically showing the relationship between the terrestrial coordinate system and the inertia measurement unit coordinate system.
2 is a view schematically showing an attitude estimation apparatus according to an embodiment of the present invention.
3A and 3B are views schematically showing the configuration of a sensor unit of the posture estimation apparatus of FIG.
Fig. 4 is a view schematically showing an example of the configuration of a rotation matrix generation unit of the posture estimation apparatus of Fig. 2;
5 is a diagram schematically showing a prediction model performing unit in a sigma point Kalman filter sequence.
6 is a diagram schematically showing a measurement model performing unit in a sigma point Kalman filter sequence.
7 is a view schematically showing a measurement model exchange unit that can be added to the posture estimation apparatus of FIG.
FIG. 8 is a flowchart schematically illustrating an attitude estimation method according to an embodiment of the present invention.
FIG. 9 is a flowchart illustrating the steps of the posture estimation method according to an embodiment of the present invention.
FIG. 10 is a flowchart illustrating a further specific step of the posture estimation method according to an embodiment of the present invention.
FIG. 11 is a flowchart illustrating in detail some other steps of the posture estimation method according to an embodiment of the present invention.
FIG. 12 is a flowchart illustrating a further specific step of the posture estimation method according to an embodiment of the present invention.
13A and 13B schematically show an example of a method of changing a measurement model in accordance with a disturbance.

Hereinafter, an attitude estimation apparatus and an attitude estimation method according to an embodiment of the present invention will be described with reference to the accompanying drawings.

1 is a view schematically showing a relationship between a ground coordinate system and an attitude estimation device. The coordinate system F 'of the posture estimation apparatus or the inertial measurement unit (IMU) 100 (hereinafter, referred to as a body frame (F)) is applied to the ground coordinate system F as a reference fixed coordinate system as shown in FIG. The transformation value T is a special orthogonal group or a rotation group corresponding to a 3 × 3 rotation matrix, The rotation matrix T value relative to the ground coordinate system F of the body coordinate system F 'is directly related to the orientation of the orientation estimation apparatus 100, .

There are two methods of estimating the attitude of the attitude estimation apparatus 100 by a deterministic method and a probabilistic method. In this embodiment, a Kalman filter, which is one of the probabilistic methods, is used And estimates the posture T of the posture predicting device 100 in real time.

2 schematically shows a configuration of an orientation estimation apparatus 100 according to an embodiment of the present invention. Referring to FIG. 2, the attitude estimation apparatus 100 of the present embodiment includes a sensor unit 110 and an operation unit 120. The posture estimation apparatus 100 may be configured such that a sensor unit 110 and an operation unit 120, which are modularized in the form of integrated circuits, are mounted on a printed circuit board.

The sensor unit 110 is a device for detecting various physical quantities so that the posture estimation apparatus 100 can estimate its posture value. The sensor unit 110 of this embodiment is configured in a combination of a plurality of sensors for measuring various physical quantities. The sensor unit 110 includes a sensor for measuring the inertia according to the motion of the attitude estimation apparatus 100, for example, an acceleration sensor 111 for measuring an acceleration and a gyro sensor 115 for measuring a change in rotation, And a geomagnetic sensor 113 for measuring a relative displacement with respect to the geomagnetic sensor 113. The various sensors constituting the sensor unit 110 are arranged to be fixed to the attitude estimating apparatus so as to be moved integrally with the attitude estimating apparatus 100.

3A to 3C illustrate examples of the sensor unit 110 of the attitude estimation apparatus 100 of the present embodiment. 3A to 3C, the sensor units 110a, 110b and 110c may be a combination of the acceleration sensor 111 and the earth magnetic sensor 113, a combination of the acceleration sensor 111 and the gyro sensor 115, (111), a gyro sensor (115), and a geomagnetic sensor (113). The sensor unit 110 may be composed of a plurality of acceleration sensors, a plurality of geomagnetic sensors, or a plurality of gyro sensors.

The calculation unit 120 may include a microprocessor as a unit for performing various mathematical operations using various physical quantity measurements measured by the sensor unit 110. [ The calculations performed by the calculator 120 may be performed by a microprocessor installed in the entire pose measuring device 100. Some calculations may be performed by the pedometer 100 and other computation devices connected by wire or wireless, .

The calculation unit 120 includes a rotation matrix generation unit 122 and an exponential coordinate system generation unit 124.

The rotation matrix generation unit 122 calculates a rotation matrix corresponding to the posture of the orientation estimation apparatus 100. [ The rotation matrix generation unit 122 extracts the angular velocity values from the measurement values provided by the gyro sensor 115, predicts the rotation matrix of the object based on the extracted angular velocity values, and outputs the rotation matrix to the acceleration sensor 111 and the earth magnetic sensor 113) based on the gravity value and the magnetic field value. The angular velocity of an object indicates the speed at which the object rotates. By integrating the measured angular velocity values, the attitude of the object can be obtained and the future posture or rotation matrix of the object can be predicted based on the measured angular velocity values. The above prediction may be inaccurate. Therefore, after the above prediction is made, the predicted rotation matrix can be corrected based on the gravity value and the magnetic field value among the provided measurement values.

The exponential coordinate system generation unit 124 performs a role of expressing the Lie algebra value as a vector by performing the exponent mapping on the rotation matrix corresponding to the Lie group and SO (3) do. A method for performing the exponent mapping is disclosed in Korean Patent Registration No. 11430379 by the inventor of the present applicant and the contents thereof are used.

Instead of setting the rotation error matrix to the state of the Kalman filter algorithm, the rotation matrix generator 122 generates a vector value having a structure suitable for the Kalman filter algorithm (i.e., a vector value obtained by mediating the logarithm value of the rotation error matrix) Since the correction operation is performed by putting it in the state of the Kalman filter algorithm, the correction operation of the rotation matrix can be performed more efficiently.

Hereinafter, the operation method and configuration of the attitude estimation apparatus 100 of the present embodiment will be described in more detail. For this purpose, terms related to the present embodiment and related art will be first defined or explained.

The posture value of the posture measuring device can be calculated from the measured values of the acceleration sensor 111 or the earth magnetic sensor 113. Hereinafter, The posture value is called the preprocessing posture, and the symbol is expressed as U. This preprocessing posture satisfies the relation of U εSO (3). Further, the attitude obtained by integrating the measurement value of the gyro sensor 115 is called a gyro integration attitude, and is represented by V in the symbol. This gyro integration posture belongs to SO (3) as well as the preprocessing posture. That is, V ∈ SO (3).

In order to explain the posture estimation method using the posture estimation apparatus of the present embodiment, various parameters will be described as follows. Definitions of the following terms are described in S. Thrun, W. Burgard and D. Fox, "Probabilistic Robotics (The MIT Press, 2005)", and are well known to those of ordinary skill in the art. Therefore, a detailed description of each of these terms is replaced with a reference to the contents of the above book. In addition, Japanese Patent Registration No. 11430379 filed by the inventor of the present applicant discloses an example of a method and an apparatus for estimating an attitude using a logarithmic filter and a Kalman filter.

x : state variable

P : Covariance matrix

T is the attitude (rotation matrix) of the inertial measurement unit to be estimated,

Q : Process noise covariance matrix

R : Measurement noise covariance matrix

X : sigma point in prediction model

Y : the sigma point in the measurement step

The posture estimation corresponds to nonlinear system dynamics. Therefore, when Kalman filter is used for attitude estimation, i) Extended Kalman Filter (EKF) which linearizes the nonlinear system in the present state function and ii) Probability density function in nonlinear system (unscented Kalman filter) which calculates a posterior probability density function (posterior pdf) using a sigma point obtained by extracting a deterministic part from a pdf, Concrete details of the non-directed Kalman filter are described in S. Julier, J. Uhlmann, and HF Durrant-Whyte, "A New Method for Nonlinear Transformation of Means and Covariances in Filters and Estimators, IEEE Trans. Recently, a model such as a cubic Kalman filter for a nonlinear system with a high degree of order has been proposed as well. Specific details are given in "Cubature Kalman filters (IEEE Trans. Automatic Control, vol.54, no. 6, 2009)" by I. Arasaratnam and S. Haykin.

Fig. 4 is a view schematically showing an example of the configuration of the rotation matrix generation unit 122 of the posture estimation apparatus 100 of Fig.

4, the rotation matrix generation unit 122 of the present embodiment includes an initial posture calculation unit 122_1_, a gyro integration posture updating unit 122_2, a prediction model performing unit 122_3, a measurement model performing unit 122_4, And a noise covariance correcting unit 122_6. The respective components of the rotation matrix generating unit 122 may be a plurality of physically separated associative devices, but one associative device And some of the components of the rotation matrix generation unit may be software installed in a computing device connected to the posture estimation apparatus of the present embodiment by wire or wirelessly.

The rotation matrix generation unit 122 of FIG. 4 may be a Kalman filter of various types including a linear Kalman filter, an extended Kalman filter, an unirradiated Kalman filter, and a volumetric Kalman filter. Hereinafter, such Kalman filters are referred to as Kalman filter series. In the present embodiment, the state variable x of the Kalman filter series is composed of exponential coordinates composed of off-diagonal components in a matrix logarithm between a pre-processing attitude and a gyro integration attitude. A description of the exponent mapping and matrix log is given in R. M. Murray, Z. Li, and S. S. Sastry, "A Mathematical Introduction to Robotic Manipulation (CRC Press)".

The initial posture calculation unit 122_1 calculates the initial posture T using the data values acquired by the respective sensors. At this time, before acquiring data from each sensor, the initial calibration value of the sensor and a constant of the Kalman filter series are set.

The gyro integration posture updating unit 122_2 performs integration from the correction value of the gyro sensor 115 and updates the gyro integration posture V from this correction value. The gyro integration posture V can be calculated either inside or outside the Kalman filter series. The gyro integral posture can include exponential mapping using Rodrigues '(Rodrigues' fomula). The gyro integration posture updating unit 122_2 may perform calibration of the gyro bias before performing the gyro integration. The calibration of the bias of the measured value of the gyro sensor 115 is performed by: i) (Ii) a change in the physical quantity vector calculated from the multi-axis sensor (acceleration sensor, geomagnetic sensor or camera) for a predetermined time is slight. Whether or not the above conditions i) and ii) are met can be determined by a short time Fourier transform, a Wavelet transform, a fast Fourier transform (FFT) or a dynamic time warping DTW). ≪ / RTI >

The prediction model performing unit 122_3 is a part for performing a prediction model or a propagation model of the Kalman filter series. The predictive model of the Kalman filter series may or may not include a gyro integration step. The state variable x and the covariance P of the Kalman filter series are updated in the prediction model.

The measurement model performing unit 122_4 is a part that performs a measurement model (correction model or update model) of the Kalman filter series. The state variable x and the covariance P are updated in the measurement model of the Kalman filter series.

The sensor calibration unit 122_5 is a part that serves to correct the calibration values of the acceleration sensor 111, the ground magnetic sensor 113 and the gyro sensor 115 by using the disturbance when it is determined that there is a disturbance. The process of correcting the calibration value of each sensor can be performed on-line during the operation of the orientation estimation device by recognizing the disturbance and using it. Correction of the calibration values of the acceleration sensor 111, the geomagnetic sensor 113 and the gyro sensor 115 may be implemented by a method of performing elliptical fitting using these measured values. As a method of calibrating each sensor, the method disclosed in Japanese Patent No. 1209571 by the inventor of the present application can be used, so that the description will be abbreviated and a duplicate description will be omitted.

The noise covariance correcting unit 122_6 serves to correct the process noise covariance Q from the estimated state variable x . The noise covariance Q may be adjusted adaptively according to the magnitude of the state variable x .

The rotation matrix generator 122 may be configured to use a Kalman filter using a sigma point, such as an unvoiced Kalman filter or a volumetric Kalman filter. Hereinafter, a Kalman filter using a sigma point is referred to as a sigma point Kalman filter system.

FIG. 5 is a diagram schematically showing a modification of the prediction model performing unit when the rotation matrix generating unit of the posture estimating apparatus of FIG. 2 uses a sigma point Kalman filter sequence.

Referring to FIG. 5, the prediction model execution unit 122_3 'includes a sigma point X calculation unit 122_31, a sigma point X decomposition unit 122_32, a state variable and a covariance update unit 122_33.

Sigma X point calculating section (122_31), calculates the sigma points X of the prediction model of the sigma-point Kalman filter series. In this case, in the prediction model of the sigma point Kalman filter series, the state variable x is made up of exponential coordinates, and the state variable can be set to all zeroes at the beginning of the prediction model. The posterior covariance P value and the process noise covariance Q are calculated using the state variable initial values thus set, and the sigma point X is calculated using these values.

Sigma point X decomposing section (122_32) is a matrix decomposition algorithm Sigma point X calculation unit (122_31) decomposing the sigma points X QR decomposition, the eigenvalues calculated in the (singular value decomposition, SVD), or Cholesky decomposition (Cholesky decompotion) .

The state variable and covariance update unit 122_33 update the state variable x and the covariance P using the calculated sigma point X. [

When the rotation matrix generator 122 uses a sigma point Kalman filter series, the measurement model of the sigma point Kalman filter series can be modified accordingly.

6 is a diagram schematically showing a modified form of the measurement model execution unit when the rotation matrix generation unit 122 of the posture estimation apparatus of FIG. 2 uses a sigma point Kalman filter sequence.

6, the measurement model execution unit 122_4 'in the case of using the sigma point Kalman filter series includes a sigma point Y calculation unit 122_41, an innovation calculation unit 122_42, a state variable and a covariance update unit 122_43, And a rear posture calculating unit 122_44.

The sigma point Y calculator 122_41 calculates the sigma point Y in the measuring step. The sigma point Y in the measurement step is calculated using the preprocessing posture U corresponding to the posture calculated from the sigma point X , the gyro integration posture V , the acceleration sensor 111 or the geomagnetic sensor 113 in the prediction step.

The innovation calculation unit 122_42 calculates an innovation term of the Kalman filter. The innovation of the Kalman filter is calculated using the previously calculated sigma point Y and the sigma point X calculated in the prediction step.

The state variable and covariance update unit 122_43 updates the posterior state variable x and the covariance P.

The posterior posture calculation unit 122_44 calculates the posterior posture T by using the posture calculated by the exponent mapping of the state variable x calculated by the state variable and the covariance update unit 122_43 and the preprocessing posture U.

The rotation matrix generation unit 122 may further include a measurement model exchange unit for switching the method of calculating the preprocessing posture U in accordance with the disturbance due to the motion of the attitude estimation apparatus 100 and the magnetic field disturbance .

7 is a diagram schematically showing a measurement model exchange unit 126 that can be added to the calculation unit 120 of the attitude estimation apparatus 100 of the present embodiment.

7, the measurement model exchange unit 126 includes a motion disturbance determination unit 126_1, a magnetic field disturbance determination unit 126_2, and a first pre-processing attitude calculation unit 126_3, a second pre-processing attitude calculation unit 126_4, Respectively.

The motion disturbance judgment unit 126_1 judges whether the disturbance due to the motion of the attitude estimation apparatus 100 is slight. The lightness of the disturbance due to motion can be detected by detecting that the measured values of the acceleration sensor 111 and the gyro sensor 115 exceed a predetermined range or by controlling the frequency such as a short time Fourier transform, a wavelet transform, a fast Fourier transform, , Or a time-frequency domain analysis algorithm.

The magnetic field disturbance determining section 126_2 determines whether there is disturbance of the magnetic field in a state where it is determined that the motion disturbance is slight. That is, whether there is disturbance of the magnetic field caused by the external magnetic field in the state where there is almost no movement of the posture predicting device 100. [ Whether the magnetic field disturbance is mild may be determined by measuring a change with time of the measured value of the geomagnetic sensor 113 and measuring whether the magnetic field disturbance exceeds a predetermined range or by comparing the measured value of the geomagnetic sensor 113 with a time- Can be determined by interpretation.

The first to third pre-processing attitude calculating units 126_3, 126_4, and 126_5 calculate the preprocessing posture U. The first preprocessing attitude calculating unit 126_3 calculates the pre-processing posture U by using the measured values of the acceleration sensor 111 and the geomagnetic sensor 113) calculates the pre-processing position U by using the measured values of the second I fed attitude calculating unit (126_4) is the pre-processing position U by using the measured value of the measured value and the gyro sensor 115, an accelerometer 111 And the third preprocessing posture calculation unit 126_5 calculates the preprocessing posture U from the gyro integration posture V. [

The first to third pre-processing attitude calculating units 126_3, 126_4, and 126_5 alternatively operate. The first pre-processing attitude calculating unit 126_3 is operated when it is determined that motion disturbance and magnetic field disturbance are mild, The pre-processing posture calculating unit 126_4 is operated when it is determined that the motion disturbance is slight but the magnetic disturbance is not slight, and the third pre-processing attitude calculating unit 126_5 is operated when it is determined that the motion disturbance is not slight .

Instead of calculating the preprocessing posture U from the measured value of the acceleration sensor 111 and the measured value of the gyro sensor 115, the second pre-processing posture calculator 126_4 may re-calibrate the geomagnetic sensor 113 processing posture calculator 126_3 and then calculates the preprocessing posture U from the measured values of the acceleration sensor 111 and the geomagnetic sensor 113 in the same manner as the first pre-processing posture calculator 126_3 It is possible. At this time, the calibration of the geomagnetic sensor 113 may be performed on-line of the attitude estimation apparatus 100.

As described above, the posture estimation apparatus 100 of this embodiment can perform the posture estimation robustly even in the presence of disturbance by calculating the preprocessing posture U in a different manner depending on the presence or absence of the disturbance and the type thereof.

Next, an attitude estimation method by the attitude estimation apparatus 100 according to the present embodiment will be described.

8 is a flowchart schematically showing a posture estimation method by the posture estimation apparatus of the present embodiment.

Referring to FIG. 8, the posture estimation method of the present embodiment includes steps of setting an initial calibration value of each sensor and a constant of a Kalman filter series, acquiring data from each sensor (S20) (S30) of calculating an initial posture from the syllable filter series (S30), updating the gyro integration posture by integrating using the gyro correction value (S40), calculating a predictive model of the Kalman filter series (S80) of correcting the calibration values of the acceleration sensor 111, the ground magnetic sensor 113 and the gyro sensor 115 in accordance with the disturbance, (Step S90) of calibrating the process noise covariance using the state variable.

Step S10 of setting an initial calibration value of each sensor and a constant of the Kalman filter series is a step of determining a constant of the Kalman filter series and an initial calibration value of each sensor before performing the attitude estimation. The initial calibration values of each sensor and the constants of the Kalman filter series are set by reading the data stored in a storage medium such as a flash memory or a hard disk of the system or an executable file or by reading the values transmitted by a wired or wireless network A method can be used.

Step S20 of acquiring data from each sensor is a step of acquiring physical quantity data measured by the acceleration sensor 111, the geomagnetic sensor 113 and the gyro sensor 115. [ At this time, the data acquired from each sensor may have different sampling rates. Therefore, the Kalman filter series performs the calculation by referring to the sampling rate of each sensor.

In the step S30 of calculating the initial posture of the posture predicting device 100 from the measured values of the respective sensors, the initial posture T of the posture predicting device is calculated. 9 shows a modification of the step of calculating the initial posture of the posture predicting apparatus 100. Referring to FIG. 9, the step S20 of calculating the initial posture T of the posture predicting apparatus includes steps of recognizing the disturbance (S22) after calculating the initial posture T in the model excluding the sensor determined to have the disturbance. Here, the disturbance may be set when the acceleration value measured by the acceleration sensor 111 is larger than the gravitational acceleration or when the magnetic field measured by the geomagnetic sensor 113 is larger than a predetermined value.

In the step of integrating using the gyro correction value and updating the gyro integration posture (S40), the gyro integration posture V is updated. The gyro integration posture V can be calculated either inside or outside the Kalman filter series. As described above, the gyro integral posture can include exponential mapping using the Rodrigues equation. FIG. 10 shows a modification of the step of updating the gyro integration posture V , which shows the calibration of the gyro bias before performing the gyro integration. 10, the step S40 of updating the gyro integration posture may be performed by using a multiaxial sensor such as the gyro sensor 115, the acceleration sensor 111, the geodetic sensor 113, And a step (S42) of determining whether the change in the measured value or the physical quantity vector of the gyro sensor 115 is mild or not. If the change in the measured value or the physical quantity vector of the gyro sensor 115 is slight, step (S43) of calculating the gyro bias and step (S44) of correcting the gyro bias are performed to calculate the gyro integration posture 45 .

In the step S50 of calculating the prediction model of the Kalman filter series, the state variable x and the covariance P are updated in the prediction model of the Kalman filter series. The predictive model of the Kalman filter series may or may not include a gyro integration step.

In the step S60 of calculating the measurement model of the Kalman filter series, the state variable x and the covariance P are updated in the measurement model of the Kalman filter series. At this time, the process noise covariance Q may be adjusted adaptively according to the magnitude of the state variable x .

The step of recognizing the disturbance S70 is performed by using the measured values of the acceleration sensor 111, the geomagnetic sensor 113 and the gyro sensor 115 to measure the motion of the orientation estimation device 100 during the operation of the orientation estimation device 100 It is judged whether there is a disturbance or a magnetic field disturbance. Whether or not the disturbance exists can be determined by conditions such as whether the change with time of the measured value of each sensor is over a predetermined range.

In the step S80 of correcting the calibration values of the respective sensors in accordance with the disturbance, when it is determined that there is movement disturbance or magnetic field disturbance, the acceleration sensor 111, the ground magnetic sensor 113, the gyro sensor 115 are corrected. The process of correcting the calibration values of the respective sensors can be implemented by a method of performing ellipse matching using the measured values of the respective sensors.

In step S90 of calibrating the process noise covariance using the estimated state variable, the process noise covariance Q is calibrated from the state variable x to be estimated. At this time, the noise covariance Q may be adjusted adaptively according to the magnitude of the state variable x .

On the other hand, when the sigma point Kalman filter series is used for the posture estimation, the step of performing the prediction model and the step of performing the measurement model may be embodied in different forms.

11 is a schematic flow chart of a step S50a of performing a prediction model in the case of using a sigma point Kalman filter series.

11, when the prediction model is performed using the sigma point Kalman filter series, the initial state variable x of the prediction model is set to 0 (step S51), and the rear covariance P calculated in the previous step and the noise covariance The sigma point X of the prediction step is calculated from Q (step S52). Also, the sigma point X is decomposed by QR decomposition, SVD decomposition or Cholesky decomposition (step S53). The state variable x and the covariance P are updated using the sigma point X (step S54).

12 is a schematic flow diagram of a step (S60b) of performing a measurement model when using a sigma point Kalman filter series.

Referring to FIG. 12, in the case of performing the measurement model using the sigma point Kalman filter series, the sigma point X , the gyro integration posture V , the preprocessing posture U calculated from the acceleration sensor 111 or the geomagnetic sensor 113, After the sigma point Y is calculated using the noise covariance Q or R (step S61), the Kalman filter innovation is calculated from the calculated sigma point Y and the sigma point X calculated in the prediction step (step S62). Further, the state variable x and the covariance P are updated using the innovation of the Kalman filter (step S63), and the posture T is calculated from the posture calculated from the exponent mapping of the state variable x and the preprocessing posture u (step S64).

On the other hand, in the above-described posture estimation method, the preprocessing posture U calculated from the acceleration sensor 111 or the geomagnetic sensor 113 may be calculated by a method of switching the measurement model according to the motion disturbance and the magnetic field disturbance.

13A and 13B are flowcharts schematically illustrating an example of a switching method of the measurement model.

Referring to FIG. 13A, the switching method of the measurement model includes a step S65 of judging whether the disturbance of movement is slight, a step S66 of judging magnetic disturbance disturbance, and a step S66 of alternately selecting the pre- And calculation steps (S67, S68, S69).

In the motion disturbance determination step S65, it is determined whether the disturbance due to the motion of the attitude estimation apparatus 100 is slight. Whether the disturbance due to the motion of the attitude estimating device is slight may be detected by detecting that the measured values of the acceleration sensor 111 and the gyro sensor 115 exceed a predetermined range or by performing a short time Fourier transform, a wavelet transform, a fast Fourier transform, Can be determined using frequency, such as warping, or time-frequency domain analysis algorithms. If the motion disturbance determiner determines that motion disturbance is large, the pre-processing posture is calculated from the gyro integration posture.

The magnetic field disturbance determination step S66 is performed when it is determined that the motion disturbance is slight in the motion disturbance determination step S65. The magnetic field disturbance determination step S66 uses the measurement value of the magnetic field sensor 113 to determine whether there is disturbance of the magnetic field. That is, whether there is disturbance of the magnetic field caused by the external magnetic field in the state where there is almost no movement of the posture predicting device 100. [ Whether the magnetic field disturbance is mild may be determined by measuring a change with time of the measured value of the geomagnetic sensor 113 and measuring whether the magnetic field disturbance exceeds a predetermined range or by comparing the measured value of the geomagnetic sensor 113 with a time- Can be determined by interpretation. If it is determined that the motion disturbance and the magnetic field disturbance are mild, a step of calculating the preprocessing posture U from the measured value of the acceleration sensor 111 and the measured value of the geomagnetic sensor 113 is performed. On the other hand, when the motion disturbance is small but the magnetic field disturbance is large, a step of calculating the preprocessing posture U from the measured value of the acceleration sensor 111 and the measured value of the gyro sensor 115 is performed.

FIG. 13B is a flow chart schematically explaining another example of the switching method of the measurement model, and is a flow chart schematically illustrating a modification of the measurement model switching method of FIG. 13A.

13B, when the motion disturbance is small and the magnetic field disturbance is large, instead of calculating the pre-processing posture U from the measured value of the acceleration sensor 111 and the measured value of the magnetic field tpstjdml, (Step S68 '), and a method of calculating the preprocessing posture U using the measured values of the acceleration sensor 111 and the gyro sensor 115 may be performed. At this time, the recalibration of the geomagnetic sensor 113 may be corrected on-line of the attitude estimation apparatus.

According to the attitude estimation apparatus and method of the present embodiment, the number or dimension of measurement sigma points in the sigma point Kalman filter series can be reduced. In addition, the bias value of the sensor unit including the acceleration sensor 111, the geomagnetic sensor 113, and the gyro sensor 115 can be corrected in real time on-line. Also, the measurement model is switched according to the state of acceleration or the disturbance caused by the change of the magnetic field, so that it has a robust effect on the disturbance. Consequently, the posture estimating apparatus and method of the present embodiment can estimate the posture T more robustly and stably.

While the present invention has been described in connection with certain exemplary embodiments, it is to be understood that the invention is not limited to the disclosed embodiments, but, on the contrary, is intended to cover various modifications and alternative constructions.

100: attitude estimation device
110:
120:
122: rotation matrix generation unit
122_3, 122_3 ': prediction model execution unit
122_4, 122_4 ': Measurement model execution unit

Claims (11)

A sensor unit including an acceleration sensor, a geomagnetic sensor and a gyro sensor,
An initial attitude calculation unit for calculating an initial orientation using data measured by the sensor unit,
A gyro integration posture updating unit for performing integration from the correction value of the gyro sensor to update the gyro integration posture,
A prediction model performing unit for performing an operation of a prediction model of the Kalman filter,
A measurement model execution unit for performing an operation of a measurement model of the Kalman filter,
A sensor calibration unit which corrects the calibration values of the acceleration sensor, the ground magnetic sensor and the gyro sensor by using the disturbance when perturbation is detected,
And a noise covariance calibration unit for calibrating a process noise covariance from the estimated state variable.
Posture estimation device. The method according to claim 1,
Wherein the Kalman filter is a sigma point Kalman filter,
Wherein the prediction model performing unit comprises:
A sigma point calculating unit for calculating a sigma point at a prediction step of a sigma point Kalman filter,
A sigma point decomposition unit for decomposing a sigma point in the prediction step;
And a state variable and a covariance update unit for updating a state variable and a covariance from the sigma point,
Posture estimation device. 3. The method of claim 2,
Wherein the Kalman filter is a sigma point Kalman filter,
Wherein the measurement model performing unit comprises:
A sigma point calculating unit for calculating a sigma point in a measurement step from a sigma point in a prediction step, a gyro integration posture, a preprocessing posture calculated from an acceleration sensor or a geomagnetic sensor,
An innovation calculation unit for calculating an innovation of a Kalman filter using a sigma point in a prediction step and a sigma point in a measurement step;
A state variable and a covariance update unit for updating the posterior state variable and the covariance using the innovation of the Kalman filter,
And a posture attitude calculation unit for calculating a posture calculated from an exponent mapping of the state variable and a posture from the preprocessing posture,
Posture estimation device. The method according to claim 1,
And a measurement model exchange unit that changes the measurement model in accordance with the presence or absence of the disturbance. 5. The method of claim 4,
The measurement model exchanging unit,
A motion disturbance judging section which judges whether disturbance is caused by motion of the posture predicting device,
And a magnetic field disturbance judging section for judging presence or absence of disturbance of the magnetic field,
A first pre-processing posture calculating unit for calculating a pre-processing posture from a measured value of the acceleration sensor and a measured value of the gyro sensor,
A second pre-processing posture calculating unit for calculating a pre-processing posture from a measured value of the acceleration sensor and a measured value of the geomagnetic sensor,
And a third pre-processing posture calculating unit for calculating a pre-processing posture from the gyro integration posture,
One of the first to third pre-processing attitude calculating sections is selectively operated depending on the presence or absence of motion disturbance and the presence of magnetic field disturbance,
Posture estimation device. An attitude estimation method for an attitude estimation device comprising an acceleration sensor, a geomagnetic sensor and a gyro sensor,
Calculating an initial orientation using data measured by at least one of the sensors,
Updating the gyro integration posture by performing integration from the correction value of the gyro sensor,
Performing an operation of a predictive model of a Kalman filter,
Performing an operation of a measurement model of a Kalman filter,
Correcting the calibration values of the acceleration sensor, the geomagnetic sensor and the gyro sensor by using the disturbance when the disturbance is detected,
And calibrating the process noise covariance from the estimated state variable.
Posture estimation method. The method according to claim 6,
Wherein the Kalman filter is a sigma point Kalman filter,
Wherein performing the operation of the prediction model of the Kalman filter comprises:
Calculating a sigma point at a prediction step of a sigma point Kalman filter,
Decomposing a sigma point in the prediction step;
And updating the state variable and the covariance from the sigma point.
Posture estimation method. 8. The method of claim 7,
Wherein the Kalman filter is a sigma point Kalman filter,
Wherein performing an operation of the measurement model of the Kalman filter comprises:
Calculating a sigma point in a measurement step from a sigma point in a prediction step, a gyro integration posture, a preprocessing posture calculated from an acceleration sensor or a geomagnetic sensor,
Calculating an innovation of a Kalman filter using a sigma point in a prediction step and a sigma point in a measurement step;
Updating the posterior state variable and the covariance using the innovation of the Kalman filter;
Calculating a posture calculated from an exponent mapping of the state variable and a posture from the preprocessing posture,
Posture estimation method. The method according to claim 6,
And changing the measurement model according to the presence or absence of the disturbance. 10. The method of claim 9,
Wherein the step of modifying the measurement model comprises:
Determining whether or not disturbance is caused by motion of the attitude estimation apparatus,
Determining whether the magnetic field is disturbed,
Calculating a preprocessing posture from the measured value of the acceleration sensor and the measured value of the gyro sensor in accordance with presence or absence of disturbance of movement and presence or absence of magnetic field disturbance from the measured value of the acceleration sensor and the measured value of the geomagnetic sensor, , Or calculating the pre-processing posture from the gyro integration posture,
Posture estimation method. 10. The method of claim 9,
Wherein the step of modifying the measurement model comprises:
Determining whether or not disturbance is caused by motion of the attitude estimation apparatus,
Determining whether the magnetic field is disturbed,
Calculating a preprocessing attitude from a measured value of the acceleration sensor and a measured value of the gyro sensor in accordance with presence or absence of motion disturbance and magnetic field disturbance; A step of calculating a pre-processing posture from the measured value of the sensor, or a step of calculating a pre-processing posture from the gyro integration posture,
Posture estimation method.

Images