Classifications

• • G—PHYSICS
  • G01—MEASURING; TESTING
  • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
  • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
  • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
  • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
  • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
  • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
  • G01C21/1654—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with electromagnetic compass
• • G—PHYSICS
  • G01—MEASURING; TESTING
  • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
  • G01C17/00—Compasses; Devices for ascertaining true or magnetic north for navigation or surveying purposes
  • G01C17/38—Testing, calibrating, or compensating of compasses
• • G—PHYSICS
  • G01—MEASURING; TESTING
  • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
  • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
  • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
  • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
  • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
  • G01C21/18—Stabilised platforms, e.g. by gyroscope
• • G—PHYSICS
  • G01—MEASURING; TESTING
  • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
  • G01P15/00—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration
  • G01P15/18—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration in two or more dimensions
• • G—PHYSICS
  • G06—COMPUTING; CALCULATING OR COUNTING
  • G06F—ELECTRIC DIGITAL DATA PROCESSING
  • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
  • G06F17/10—Complex mathematical operations

Definitions

• the present invention relates to a method for estimating the orientation of an object in space, with or without its own acceleration and with or without magnetic disturbance, and to a device capable of allowing the estimation of the orientation. implementing such a method.
• the obtaining of the orientation generally requires the implementation of several sensors, forming part of a set designated by motion capture device, also designated by central attitude.
• MEMS sensors Micro-Electro-Mechanical Systems
• electronic microsystems can be used to build this plant, they have the advantage of being small and low cost.
• attitude centers that use together accelerometers and magnetometers, which make it possible to reconstruct the movements with three degrees of freedom, that is to say movements whose own accelerations and Magnetic disturbances are respectively negligible compared to the Earth's gravity field and the Earth's magnetic field.
• this hypothesis is not respected, that is to say that one can not neglect the own acceleration or magnetic disturbances, movements have six or nine degrees of freedom. It is therefore impossible, using an attitude center using only accelerometers and magnetometers to estimate the orientation of the object in motion.
• the diversification of the applications of motion capture imposes to overcome these constraints, It was then envisaged to use additional sensors, particularly to jointly use gyrometers, accelerometers and magnetometers.
• the measurements from these sensors are composed of two parts: an informative part directly related to the orientation of the object in motion and a disturbing part whose nature depends on the sensor considered.
• these are clean accelerations for the measurements provided by the accelerometers, magnetic disturbances for the measurements delivered by the magnetometers and the bias for the gyrometers. These disturbances lead to an erroneous estimation of the orientation.
• Extended Kalman Filter Extended Kalman Filter
• EKF Extended Kalman Filter
• the quality of the measurements injected into the filter is of great importance, and in particular the confidence that one gives to their value.
• the measurements include an informative part directly related to the orientation of the moving object and a disturbing part whose nature depends on the sensor considered. In the first order, these are clean accelerations for the measurements provided by the accelerometers, magnetic disturbances for the measurements delivered by the magnetometer and the bias for the gyrometers. It is also necessary to take into account the noise of measurement, however this one is conventionally treated in the filter. There are currently several methods for dealing with disturbances.
• the information provided by the disturbance measure is therefore not taken into account for the estimation of the orientation.
• the orientation estimate is based only on the measurements provided by the other sensors.
• the observer does not have enough information to propose a correct estimate of the orientation.
• the process does not neglect disturbances, which does not distort the estimate; he estimates them permanently. If they exist, it does not reject the associated measurement or measurements, as is the case in other estimation methods. Moreover, it does not integrate them in the state vector or in the measurement model, which simplifies the model and does not lead to situations where estimation becomes impossible. It is therefore expected to estimate the orientation, and possibly to estimate the disturbances, in two successive stages.
• the observer is thus provided with measurements of accelerometers, magnetometers and gyrometers as close as possible to ideal conditions for estimating the orientation: that is to say without proper accelerations, without magnetic disturbances and without bias, respectively. For this, it is expected to use the orientation estimated at the previous instant as additional information to perform preprocessing measures.
• the estimation method according to the invention therefore makes it possible to extract measurements from the sensors, the orientation of the object in an optimal manner, whatever the movement considered.
• This method is also simple to implement and has only a small number of adjustment parameters.
• the observer is advantageously an extended Kalman filter. It is possible to estimate the disturbances, in particular the natural accelerations, which makes it possible, by integration and double integration, to go back to the speed and to the position of the object respectively.
• the main subject of the present invention is therefore a method for estimating the orientation of an object in space at the instant k using the measurements of the total acceleration, of the magnetic field and the speed of rotation of said object.
• Step A advantageously comprises: A1- a pretreatment of the rotational speed measurements, A2- a detection of the existence or not of a disturbance at the insta nt k in said measurements of the total acceleration and magnetic field,
• Step A1 consists in subtracting from the rotational speed measurements an average bias determined during a preliminary initialization step. This average bias can be obtained by immobilizing the means providing the rotational speed measurements for a given time and calculating the average of the values of the rotational speed measurements on each axis. In the case of an attitude center carried by a person, this immobilization is to remove the central of the person to overcome the inevitable tremors of the person.
• the pretreatment step A2 of the acceleration and magnetic field measurements may comprise: a step A2.1 consisting of a comparison test of the standard of the measurements of the total acceleration with that of the gravitational field, if the absolute value the difference between the standard of accelerometric measurements at time k and that of the gravitational field is less than a predetermined threshold, it is considered that the accelerometric perturbation is zero, otherwise it is considered that there is a disturbance, the perturbation being equal on each axis, the difference between the measurement of the total acceleration on each axis at time k and the undisturbed accelerometric measurement estimated at time k, and - a step A2.2 consisting of a comparison test from the standard of magnetic field measurements to that of the Earth's magnetic field, if the absolute value of the difference between the standard of magnetic field measurements and that of the magnet field If the earthquake is below a predetermined threshold, it is considered that the magnetic disturbance is zero, otherwise the magnetic disturbance is considered to be equal, on each axis, to the difference between the magnetic field measurement on each axi
• step A2.1 is provided with an additional test on the estimated disturbance at instant k-1: in the case where the absolute value of the difference between the standard of the measurements of the total acceleration and that of the gravitational field at the moment k is below the predetermined threshold, it is checked whether the norm of the accelerometric perturbation estimated at time k-1 is below a predetermined threshold, if this test is positive, it is considered that the accelerometric perturbation is effectively zero at time k, and / or at step A2.2, an additional test is provided on the magnetic disturbance estimated at time k-1: in the case where the absolute value of the difference between the standard of the magnetic field measurements and that of the earth's magnetic field is lower than the predetermined threshold, it is checked whether the absolute value of the magnetic disturbance estimated at time k-1 is below a predetermined threshold, if this test is positive it is considered that the magnetic disturbance is effectively zero at time k.
• y A k , y M k , y G ⁇ the measurements obtained at the end of the preprocessing step which are called estimated undisturbed measurements: - the detection of clean accelerations is carried out solely through the standard of accelerometric measurement; If this norm is different from the Go (Earth Gravity) norm on at least one of the measurements of a sliding window of duration T A then the measurement at the current time is considered to be disturbed; - the detection of magnetic disturbances is carried out in a similar way: o if the norm of the magnetometric measurement is different from the norm of Ho (terrestrial magnetic field) on at least one of the measurements of a sliding window of duration T M o OR if the angle between the magnetometric measurement and the opposite of the undisturbed accelerometric measurement - y A is different from the angle between the vectors Go and H 0 , then the measurement at the current time is disturbed magnetically.
• TA may be a parameter of constant value while the value of T M may be related to the speed of movement.
• the observer used in step B is preferably an extended Kalman filter, which is fast and simple.
• Step B of estimating the orientation from the measurements estimated at time k may comprise:
• the state vector used in the Kalman filter can contain the elements of angular velocity and orientation quaternion.
• the state vector used in the extended Kalman filter advantageously contains only the elements of the orientation quaternion, which makes it possible to simplify the structure of the state and measurement models.
• the present invention also relates to an attitude center comprising means capable of providing acceleration measurements, magnetic field measuring means, and means for measuring the speed of rotation along three axes of the space, said means being intended to be integral in motion of an object, and means for estimating an orientation at time k on the basis of measurements provided by said measuring means, said estimating means comprising:
• pretreatment means for pretreatment of said acceleration, magnetic field, and rotational speed measurements, said pretreatment means being able to detect the existence of a disturbance in said measurements; and delivering estimated undisturbed accelerometric measurements, estimated undisturbed magnetometer measurements, and unbiased rotational velocity,
• This observer can be an extended Kalman filter.
• the attitude control unit according to the present invention may also comprise means for calculating an average bias of the means for measuring the speed of rotation during an initialization step of the control unit.
• the pretreatment means may comprise means for detecting the existence of an own acceleration in the acceleration measurements and means for detecting the existence of magnetic disturbances in the magnetic field measurements.
• the attitude unit according to the invention may also comprise means for estimating the proper acceleration and for calculating the speed and the position of the object.
• the means capable of providing measurements of the total acceleration, magnetic field measurements, and measurements of the speed of rotation along three axes of the space are advantageously MEMS sensors.
• FIG. 1 is a flowchart of the method according to the present invention at time k
• FIGS. 2A to 2C show detailed flowcharts of a step of pretreatment of the measurements of an accelerometer, a gyrometer and a magnetometer respectively, according to the present invention.
• a central attitude comprising sensors able to provide measurements of the total acceleration, magnetic field and rotation speed along the three axes of the space.
• the sensors are advantageously MEMS sensors offering a reduced cost and a small footprint. It may be, for the measurement of acceleration, for example a tri-axis accelerometer or three single-axis accelerometers providing a measurement on each of the axes.
• it may be a tri-axis magnetometer or three single-axis magnetometers.
• it may be, for example, three single-axis gyrometers or preferably two bi-axis gyrometers.
• the triaxes can be aligned or not, in the latter case the relative orientation between the axes must be known.
• we will designate the accelerometer or accelerometers, by an accelerometer, the magnetometer (s) by a magnetometer and the gyrometer (s) by a gyrometer.
• YM tri-axis measurement of the magnetic field supplied by the magnetometer
• YG tri-axis measurement of the speed of rotation provided by the gyrometer
• R rotation matrix
• G 0 earth gravity field (vector 3x1)
• H 0 Earth's magnetic field (vector 3x1)
• ⁇ angular velocity
• a natural accelerations
• d magnetic disturbances
• b gyrometer bias
• v A accelerometer measurement noise
• v M measurement noise of the magnetometer
• VQ measurement noise of the gyrometer.
• the orientation is estimated with respect to a reference frame, entirely defined by the data of the vectors Go and Ho.
• the geocentric reference is defined by the vectors
• each of these measures includes a first part
• a pretreatment step is provided before use in a processing step for providing an estimate of the orientation.
• the method according to the present invention comprises a step 100 of initialization of the attitude center, a step 200 of preprocessing the measurements provided by the sensors and a third processing step 300 by the observer.
• the observer used in the measurement processing step is an extended Kalman filter, which is simple, robust and quick to implement.
• a Kalman filter includes a state model defining the temporal and dynamic evolution of states, and a measurement model that allows to link sensor measurements and states.
• the state vector of the Kalman filter is composed according to a first modeling of the three elements of the angular velocity and the four elements of the quaternion defining the orientation.
• the associated state and measurement models can be respectively:
• W ⁇ modeling noise
• a state vector containing only the elements of the quaternion which is only of dimension 4, whereas it is of dimension 7 in the first modeling.
• the gyrometric measurement is then injected directly into the state model and the measurement vector contains only the accelerometer and magnetometer measurements.
• This second modeling makes it possible to simplify the structure of the state and measurement models since their dimension is directly reduced. Moreover, the number of adjustment parameters, in particular the elements of the covariance matrices of the modeling noise, the measurement noise and the estimation error of the state vector, is also restricted, which facilitates the setting of implementation of this method.
• the estimation results obtained in this way are of similar precision to those obtained using the first modeling.
• the initialization step 100 provides for estimating the average perturbation of the gyrometer.
• This disturbance b which is in fact the bias of the gyrometer, varies between two extreme values.
• the initialization step (k 1): Let we know the proper acceleration ai and the magnetic disturbance di at the initial moment, then the initialization step involves the determination of the state vector Xi: the angular velocity is assumed to be zero at the initial moment, the quaternion is determined by optimization using the acceleration and magnetic field corrected for the perturbations ai and di; Either we know the orientation in the initial state, in this case we can deduce ai and di.
• the stationary attitude unit is maintained for a predetermined time, for example about one second, and the average of the output values of the gyrometer is calculated on each axis.
• This estimate of the average mean bias b is subsequently subtracted from each measurement of the gyrometer during the pretreatment step 200, which makes it possible to minimize the influence of the bias and thus to improve the accuracy of the results obtained.
• This estimate of the average mean bias b preferably takes place at the beginning of each acquisition. It is also possible to renew this estimate during periods of immobility.
• FIGS. 2A to 2C the details of the steps of the method according to the invention can be seen.
• step 200 the measurements delivered by the three sensors are preprocessed in three steps 210, 220 and 230. According to the context of use of the method and the desired accuracy, several alternative embodiments are possible.
• the first step 210 is identical in all the embodiments.
• the pretreatment of the measurement y ⁇ , k of the gyrometer takes place.
• this pretreatment of the measurement y0 , k is obtained by subtracting the average bias b mmen from the actual measurement, a preprocessed measurement of the gyrometer at the instant k designated by y G k is obtained.
• the acceleration is given in a multiple of GB (Earth magnetic field), and the magnetic field is given in a multiple of Ho for simplification purposes.
• step 220 two tests for detecting the existence of accelerometric (step 220) and magnetometric (step 230) perturbations are advantageously carried out in parallel.
• step 220 the pre-processing of the measurements delivered by the accelerometer y Alk at time k takes place.
• This step 220 comprises a first substep 220.1 for detecting the existence or not of a disturbance, ie of an own acceleration a, and a second substep 220.2 for constructing a pretreated measurement of the acceleration y A k on the basis of the orientation estimated at the previous instant k-1.
• the norm of the measure y ⁇ k is compared to the norm of the gravitational field (as a reminder, one works in a multiple of GB), one makes a comparison with 1: If y * I ⁇ l ⁇ a A>
• AI ⁇ 'A The comparison of the norm of the eigenaccess estimated at time k-1, kA , at ⁇ A, advantageously makes it possible to exclude particular cases for which the first test would not suffice. Indeed, it is considered that if at the moment k-1 the acceleration owns a high value, ie greater than ⁇ A, it is unlikely that at the moment k the acceleration is less than ⁇ A. A and ⁇ A are for example equal to 0.04 and 0.2 respectively. This second test therefore improves the accuracy of the estimation of the undisturbed measurement y ⁇ k and thus the estimation of the orientation.
• step 220.2 a new acceleration measurement is constructed using the estimated orientation q kA , estimated at the previous instant.
• the undisturbed accelerometric measurement estimated at time k is then written using the measurement model: .
• step 230 similar to step 220, the preprocessing of the measurements delivered by the magnetometer YMM at time k takes place.
• This step 230 comprises a first substep 230.1 for detecting the existence or not of a magnetic disturbance d, and a second substep 230.2 of constructing a pretreated measurement of the magnetic field y M k on the basis of the orientation estimated at the previous instant k-1.
• step 230.1 in order to detect the existence or not of a magnetic disturbance d, the norm of the measurement y Mtk is compared with the norm of the magnetic field (as a reminder, one works in a multiple of H 0 ), on therefore makes a comparison with 1:
• a new magnetic field measurement is constructed using the estimated orientation q kA , estimated at the previous instant.
• the comparison provided for in step 220 is carried out on a window ending at the instant f0: if a proper acceleration is detected (norm of the accelerometric measurements different from the standard of Go to a 4 on at least one of the measurements of the window [fc - T A ; t k ]) then the undisturbed accelerometric measurement is constructed thanks to the orientation estimated at the previous instant; otherwise, the undisturbed accelerometric measurement is equal to the measurement entering the preprocessing phase (sensor measurement).
• the value of the own acceleration is calculated from the sensor measurement. Typical values given by way of example only for a A and TA are 0.2 g and 0.4 s.
• the own acceleration can be calculated systematically, even if thresholds are not exceeded by the same formula as in the first variant embodiment:
• the disturbance detection test is carried out of the magnetometric signal provided in step 230: if a magnetic disturbance is detected (standard of the magnetometric measurements different from the norm of H 0 to a M on at least one of the measurements of the window [t k -
• the undisturbed magnetometric measurement is constructed by the orientation estimated at the previous instant. Otherwise, the undisturbed magnetometric measurement is equal to the measurement entering the pre-processing phase (sensor measurement). The value of the magnetic disturbance is then calculated from the sensor measurement. Typical values given only as an example for a M , and TM are respectively 0.1h and 0.5s.
• the magnetic disturbance can be calculated systematically, even if thresholds are not exceeded by the same formula as in the first variant embodiment:
• steps 202 and 203 are advantageous to be performed in parallel.
• a third embodiment makes it possible to improve the accuracy of the detection, when this is necessary, and that the device is provided with calculation and storage means that are sufficient to use trionometric functions.
• the advantage provided by the parallelism of the detection calculations is dispensed with and it is more advantageous to carry out the magnetic disturbance test after the presence test of a proper acceleration.
• u k angle (-y ⁇ k , y uk ) is also calculated and the measured angle between the Go and HQ vectors is recorded as Uo. This parameter can be calculated during step 100 of initialization.
• the magnetic disturbance detection test is carried out as follows: if a magnetic disturbance is detected (standard of the magnetometric measurements different from the norm of H 0 to or near M or angle u k different from M 0 to a u close to at least one of the measurements of the window [Î H -T M ; t k ]), then the undisturbed magnetometric measurement is constructed by virtue of the orientation estimated at the previous instant. Otherwise, the undisturbed magnetometric measurement is equal to the measurement entering the pre-processing phase (sensor measurement). The value of the magnetic disturbance is then calculated from the sensor measurement.
• T M j as t
• Typical values given by way of example only for a M , a u , TMjast and TM_SIOW are respectively 0.1 h, 10 °, 0.5 s and 3 s.
• step 300 the preprocessed measurements y A ⁇ , y M k , y G k are used by the observer.
• an extended Kalman filter is used in its factored form.
• Step 300 comprises the following steps: a) A priori estimation of the state vector, b) A priori estimation of the measurements, c) Calculation of the gain K k of the Kalman filter and the innovation l k , d) Correction of the state estimated a priori.
• Steps a) to d) will be described in detail below.
• a priori estimation of the state vector at time k is performed from the a posteriori estimation of the state vector at time k-1.
• the estimation of the prior state vector is given by: with X ⁇ 1 : a posteriori estimation of the state vector at time k-1,
• % l a priori estimation of the state vector at time k
• step c) the gain K k is calculated and the innovation l k , the innovation is obtained by subtracting the measures estimated a priori from the pre-processed measures.
• step d the estimated state is corrected a priori with the gain and the innovation. This correction gives a posteriori estimate of the orientation q k at time k.
• the method according to the present invention offers the advantage of providing the observer with measurements close to undisturbed measurements, consistent with the measurement model. The influence of disturbances on the estimation of the orientation is therefore greatly reduced.
• the joint use of the accelerometer, magnetometer and gyrometer measurements makes it possible to reduce the influence of measurement errors (measurement noise, remaining disturbances, remaining gyrometer bias) on the estimated orientation.
• the method according to the present invention makes it possible to estimate the orientation, but also the natural accelerations and the magnetic disturbances at each sampling step, whatever the movement achieved up to nine degrees of freedom.
• the implementation of this method is very simple since it relies on the use of elementary bricks: value tests, analytical calculations, extended Kalman filter.

Applications Claiming Priority (2)

Publications (1)

Family

ID=40383542

Family Applications (1)

Country Status (7)

Families Citing this family (20)

Family Cites Families (11)

• 2008
  • 2008-07-18 FR FR0804116A patent/FR2934043B1/fr not_active Expired - Fee Related
• 2009
  • 2009-07-17 KR KR1020117003770A patent/KR101693898B1/ko active IP Right Grant
  • 2009-07-17 EP EP09797515A patent/EP2313739A1/de not_active Ceased
  • 2009-07-17 CN CN200980132305.5A patent/CN102308183B/zh not_active Expired - Fee Related
  • 2009-07-17 WO PCT/EP2009/059225 patent/WO2010007160A1/fr active Application Filing
  • 2009-07-17 US US13/054,259 patent/US20110208473A1/en not_active Abandoned
  • 2009-07-17 JP JP2011517951A patent/JP5861235B2/ja not_active Expired - Fee Related

Non-Patent Citations (2)

Also Published As

Similar Documents

Legal Events