JP6147446B1 - Inertial sensor initialization using soft constraints and penalty functions - Google Patents

Abstract

An initialization method for an inertial sensor that estimates the starting orientation and speed without requiring the sensor to start in a quiescent state or in a known location or orientation. Initialization uses a motion pattern encoded as a set of soft constraints that are expected to be mostly observed during the initialization period. Penalty metrics are defined to measure the deviation of the calculated motion trajectory from the soft constraints. The differential equation of motion for the inertial sensor is solved by the initial state as a variable, and the initial state that minimizes the penalty metric is used as an estimate of the actual initial state of the sensor. Application specific soft constraints and penalty metrics are selected based on the type of motion pattern expected for the application. Exemplary cases include applications that have relatively little motion during initialization and applications that have approximately periodic movement during initialization.

Description

  One or more embodiments relate to motion capture and sensor initialization associated with motion capture that can be coupled to a user or device. More particularly, but without limitation, one or more embodiments allow for initialization of the inertial sensor using soft constraints and a penalty function to estimate the initial state of the inertial sensor device. . Initialization is generally performed at some point based on data captured from the device during the initialization time interval and a set of soft constraints on the movement of the device during the interval.

  Inertial sensors are commonly used for motion tracking and navigation. They offer the advantage that they can be used to estimate the motion of an object without the need for external inputs or references. These systems typically include an accelerometer for measuring linear forces and a gyroscope for measuring rotational movement.

  Such a system has been used for many years as a navigation support device for ships and aircraft. More recently, with the availability of inexpensive MEMS sensors, inertial sensors are increasingly being incorporated into consumer products such as smartphones.

  A pure inertial sensor cannot detect uniform linear motion, which is basically a result of the relative principle. Nor can they determine absolute position or orientation. In effect, inertial sensors can be used to track changes in position and orientation. As motion tracking or navigation devices, they rely on dead reckoning techniques to estimate new positions and orientations from known starting conditions and from changes detected by inertial sensors.

  Therefore, applications using inertial sensors must use some method to determine the initial state of the sensor before measuring changes in position and orientation via inertial data. Existing systems typically use one or both of two schemes for this initialization.

  One existing method of initializing the inertial sensor is to place the sensor in a known position and orientation and start the motion tracking for a period of time (with no linear or rotational motion). It guarantees that it is in a dormant state. As a result, the initial position and orientation of the sensor can be provided directly to the motion tracking algorithm, and the initial linear and angular velocities are set to zero to reflect the initial state of rest. Can do. This scheme is quite simple, but may not be applicable in some situations. Sometimes the initial position or orientation is not known. In some applications, it may not be feasible to ensure a period of dormancy before beginning motion tracking.

  Existing systems that require the sensor to be inactive before tracking motion can sometimes be used to estimate the initial tilt of the sensor relative to the vertical line by using an accelerometer as a sensor for the gravity vector. Can be obtained. This technique is only applicable when the sensor is at rest and there is negligible vibration, otherwise the accelerometer data will reflect a combination of true acceleration and gravity. Become.

  A second existing method of inertial sensor initialization is to supplement the inertial sensor data with other sensor data supplies. For example, GPS systems are often used in the context of inertial sensors to provide a data supply with absolute position information. A magnet may be used in the context of an inertial sensor to provide a data feed having absolute orientation information. A hybrid system of a combination of these inertial sensors and non-inertial sensors is generally used for navigation. For example, the GPS system may be used as the main navigation tool, in which case inertial sensors provide extrapolation between GPS updates and backups in cases where GPS signals are temporarily unavailable. Yes.

  A drawback of hybrid systems is the cost and complexity of additional sensor hardware and software. Specifically, in the case of GPS (or similar location detection systems such as those using cellular phone signals), a GPS receiver must be integrated into the sensor hardware. This may not be practical or may not work at all in certain environments, such as indoors.

  In summary, known systems for initializing inertial sensors generally rely on placing the sensor in a quiescent state and a known state, or supplementing the inertial sensor with additional sensor data feeds. Both schemes have limitations and disadvantages in certain applications. Accordingly, there is a need for an inertial sensor initialization method that can be used in relatively general situations and without the need for additional sensor hardware.

  Embodiments of the present invention determine the initial state of the inertial sensor device based on data captured from the inertial sensor device during the initialization time interval and a set of soft constraints on the motion of the inertial sensor during the interval. Relates to a method for estimating at a certain point in time. These constraints may be referred to as soft constraints because, in some embodiments, these constraints are expected to be largely respected for device movement during the initialization time interval. . Although slight deviations from these soft constraints can be expected, no major deviations can be expected. Different embodiments of the present invention may use different soft constraints that potentially reflect typical motion patterns expected in various applications using these different embodiments.

  One or more embodiments of the present invention may not require the inertial sensor to be dormant or in a particular orientation throughout the initialization time interval. Some further embodiments may not require that data captured from inertial sensors be supplemented by other non-inertial data such as GPS readings. Thus, embodiments of the present invention use inertial motion patterns expected during initialization as a set of soft constraints, so that inertial conditions can be achieved under a wide range of conditions and without additional sensor hardware. It can be used to estimate the initial state of the sensor. For example, one or more embodiments of the present invention provide an initial orientation or initial speed for a device in continuous use where it is impractical to require the device to remain in a dormant state for a dedicated initialization procedure. Can be used to estimate. Such use cases are for example for sports equipment with integrated inertial sensors that are impractical to disturb the user of the device during the game, due to initialization of the orientation or speed of the device. Could be applied.

  Embodiments of the present invention may use one or more penalty metrics to quantify the deviation of the actual motion trajectory of the inertial sensor device from the set of soft constraints. In applications using such embodiments, the penalty metric used by one or more embodiments of the present invention, as well as the soft constraints themselves, may be selected based on the expected motion pattern. .

  In one or more embodiments, the initial estimate of the inertial sensor can be calculated by minimizing the penalty metric. In effect, this procedure finds an initial state that generates a motion trajectory (which may include position and velocity trajectories) close to the soft constraints defined for such an embodiment. In conventional inertial motion tracking applications in existing technology, the initial state of the inertial sensor is known, and the sensor motion trajectory is a well-known differential equation of motion for the inertial sensor (analytically or It is calculated by integrating (numerically). The scheme used in one or more embodiments of the present invention, in a sense, is the opposite of the normal motion tracking scheme and is found by minimizing the deviation of the motion trajectory from the soft constraints. The initial state may be treated as a variable to be handled.

  The determination of the initial state that minimizes the penalty metric can be performed by any of a set of known techniques for function minimization. These include direct analytical techniques (sometimes used in the case of simple functional forms) and various numerical methods for function optimization. In addition, the embodiment of the present invention may apply an approximate minimization method in order to obtain an estimated value that is inaccurate but useful in the initial state. Such approximate methods can be used, for example, when initialization calculations are performed on embedded processors with relatively small computing power, or when an approximate estimate is urgently needed. Could be used if

  In one or more embodiments of the present invention, the soft constraints on inertial sensor motion are such that the sensor position remains relatively unchanged during initialization and the initial velocity of the sensor is relatively small. It may include the constraint. These embodiments may use a simple penalty metric that consists of a net change in inertial sensor position across the initialization period. A simple penalty metric of this type is that, for example, a player is moving his or her golf club (including an integrated or wearable inertial sensor) at the time of addressing, but the club head is It could be used in one or more embodiments in golf swing analysis applications that never move very far away from the ball, and that do not move very rapidly when addressing.

  In one or more embodiments, the initial orientation of the sensor can be calculated directly by using a technique that separates the differential equation of motion into an orientation equation and a position and velocity equation. This separation of differential equations yields a net position change equation that can be analytically minimized to give an initial estimate (penalty metric). This analytical solution may provide benefits in applications that cannot provide time or computational resources due to the iterative numerical minimization scheme.

  In one or more embodiments of the present invention, the soft constraint on the motion of the inertial sensor may include one or more constraints that signal that the motion is approximately periodic during the initialization interval. . The penalty metric in one or more embodiments may include a change in net position and a change in net velocity over a single selected period of motion. Such constraints and penalty metrics include, for example, baseball swing analysis where a batter moves his bat in quasi-periodic back-and-forth motion or in repetitive circular motion while waiting for a pitch. Could be used in one or more embodiments in an application.

  In one or more embodiments of the present invention, an estimate of the period of motion may be calculated in a preliminary processing step prior to minimizing the penalty metric. One or more embodiments may include a period estimation step using a discrete Fourier transform (DFT) on the inertial sensor data during initialization to identify the dominant frequency of motion (and thus the dominant period). The use of a preliminary period estimation step provides the benefit that the period can be calculated dynamically from the observed movement. Thus, this may be an adaptive method used by one or more embodiments that adjust soft constraints (especially the period) to a particular pattern of movement observed for a particular event. .

  In one or more embodiments that use a penalty metric that includes a net position change or a net speed change over time, the separation of the differential equations may result in net speed and position nets during this initialization period. It may be applied to obtain a change equation. In one or more embodiments, these equations can be solved analytically in turn, and the initial orientation can be calculated first to minimize the net velocity change. And then the initial velocity can be calculated to minimize the net position change.

  In one or more embodiments of the present invention, collecting sample data from the inertial sensor over a capture time interval that includes the activity of interest in determining an estimate of the initial orientation and initial velocity of the inertial sensor. A method may be utilized, in which case the sample data includes accelerometer data and rate gyro data. The accelerometer data and rate gyro data are each a time-series three-dimensional vector, and in some embodiments, the inertial sensor may provide data for less than three axes of the gyro or accelerometer, This data can always be embedded in a three-dimensional vector. The orientation of the inertial sensor at any point in time is an orthogonal transformation in three-dimensional space, which can be expressed in various forms such as a 3 × 3 matrix or a unit quaternion. The velocity and position of the inertial sensor at any point and time are each a three-dimensional vector. The initialization time interval is selected within the capture time interval. A set of soft constraints is defined in terms of inertial sensor position and velocity trajectories during the initialization interval. Each soft constraint defines a set of possible trajectories of position and velocity vectors. In one or more embodiments of the present invention, the soft constraints represent an approximation of a representative pattern of the position and velocity trajectory of the subject's activity during the initialization interval. A set of penalty metrics is defined to quantify the deviation of the calculated position and velocity trajectory of the inertial sensor from soft constraints. Inertial sensor initial orientation and initial velocity estimates at the start of the initialization interval, the initial orientation and initial velocity estimates integrate the sample gyro and accelerometer data into the orientation, velocity, and position over the initialization interval. Therefore, when used as an initial state, it is calculated to minimize the penalty metric. In one or more embodiments, the integration uses a combined motion differential equation of inertial sensors that relate the time derivatives of orientation, velocity, and position to each other and to the measured accelerometer and gyro data. doing. In one or more embodiments, a differential equation is used in which the time derivative of the orientation is equal to a distorted symmetric matrix that represents the cross product between the orientation × gyro vector. A second differential equation is used where the time derivative of the velocity vector is equal to the orientation x accelerometer vector and this amount is added to the gravity vector. The gravity vector term reflects the fact that the inertial sensor accelerometer measures the apparent movement in relation to free fall, not true acceleration. A third differential equation is used in which the time derivative of the position vector is equal to the velocity vector.

  In one or more embodiments of the invention, the soft constraint includes a requirement that the change in position between the start and end of the initialization interval is zero, which means that the inertial sensor is effectively This reflects what is expected to remain in the same location or in a limited area of space over the initialization interval. In one or more embodiments, the penalty metric includes a metric defined as the absolute value of the position difference between the end of the initialization interval and the start of the initialization interval. This metric measures the deviation of the position trajectory from the soft constraint of zero net motion of the inertial sensor. In one or more embodiments, the inertial sensor initial velocity estimate is set to zero and a penalty metric minimization is used to estimate the initial orientation of the sensor.

  In one or more embodiments of the invention, the soft constraints include the requirement that the position and velocity of the inertial sensor have periodicity during the initialization interval. In one or more embodiments, calculating a frequency domain signal for each axis of the gyro using a discrete Fourier transform on the corresponding axis of the time series of rate gyro data during the initialization interval, An estimate of the dominant period of motion during the initial interval is acquired. The peak magnitude of the transformed frequency domain signal on each axis is calculated and the maximum peak magnitude across all axes is obtained. The frequency at which this maximum peak magnitude occurs is used as the dominant frequency of motion during the initialization interval. The dominant period of motion is calculated as the reciprocal of the dominant frequency. In one or more embodiments, the penalty metric is a velocity penalty metric that is the absolute value of the change in velocity over one movement period and a position that is the absolute value of the change in position over one movement period. Including penalty metrics. A specified period within the initialization interval is selected with a length equal to the dominant period of motion. In one or more embodiments, an initial orientation estimate is calculated and then a pre-calculated initial orientation estimate by minimizing a velocity penalty metric over a first specified period of time. Is used to minimize the position penalty metric over a specified period of time, thereby calculating an estimate of the initial velocity of the sensor.

  The foregoing and other aspects, features, and advantages of the concepts conveyed throughout this disclosure will become more apparent from the following more specific description thereof, presented in connection with the accompanying drawings in which:

Fig. 3 shows a flow chart of an embodiment of the present invention showing the main steps in a method for generating an initial orientation estimate of an inertial sensor device. FIG. 2 shows a detailed view of two of the steps of FIG. 1 showing selection of data captured from the inertial sensor and an initialization interval. FIG. 2 shows a detail view of two more of the steps of FIG. 1 showing the definition of soft constraints on position and velocity and the definition of penalty metrics for deviation from these soft constraints. FIG. 2 shows a detailed view of one embodiment of the last step of the flowchart of FIG. 1 that calculates the initial state by integrating a differential equation of motion and applying a penalty metric to the resulting position and velocity. FIG. 6 illustrates one embodiment of a method in which a penalty metric is comprised of a change in position over an initial interval and the initial velocity estimate is set to zero. FIG. 6 illustrates one embodiment of a method that uses a particular technique to calculate an optimal initial orientation by extracting the initial orientation from a motion equation. FIG. 6 further illustrates the technique of FIG. 6 illustrating one embodiment of a method for obtaining a direct solution of an initial orientation estimate. FIG. 6 illustrates a flowchart of one embodiment of a method for applying a soft constraint to a position and velocity that requires periodic motion during an initialization interval and estimating the period of this periodic motion using a discrete Fourier transform. . FIG. 9 shows a detailed view of two of the steps of FIG. 8 showing the periodicity constraint and the definition of a penalty metric for deviation from this periodicity constraint. FIG. 9 shows a detailed view of two more of the steps of FIG. 8 illustrating the use of the discrete Fourier transform to calculate the dominant period during the initialization interval. FIG. 9 shows a detailed view of one embodiment of the last two steps of FIG. 8 showing the calculation of the first optimal initial orientation estimate followed by the calculation of the optimal initial velocity estimate.

  Hereinafter, a method of determining the initial value of the inertial sensor and the estimated value of the initial speed will be described. In the following illustrative description, numerous specific details are set forth in order to provide a more thorough understanding of the concepts described throughout the specification. However, it will be apparent to one skilled in the art that the embodiments of the concepts described herein may be practiced without incorporating all aspects of the specific details described herein. . In other instances, detailed descriptions of specific embodiments well known to those skilled in the art have been omitted so as not to obscure the disclosure. The reader should note that while example concepts of the invention have been described throughout this disclosure, the entire scope of the claims and any equivalents define the invention.

  FIG. 1 shows a flowchart of one embodiment of a method for determining an initial orientation and initial velocity of an inertial sensor. At 101, sample data is captured from an inertial sensor. This sample data consists of one or more time series of measurements across the capture time interval, where the capture time interval includes the activity of interest. At 102, an initialization interval is selected within this capture time interval. This initialization interval will be used to calculate the initial orientation and initial velocity. At 103, separately, a set of soft constraints on the position and velocity of the inertial sensor is defined. At 104, a penalty metric is defined for measuring deviations in calculated position and velocity from soft constraints. At 105, sample data from the initialization interval and the penalty metric are used to calculate an estimate of the initial orientation and initial velocity by minimizing the penalty metric across the initialization interval.

FIG. 2 shows a detailed view of the sample data collection step 101 and the initialization interval selection step 102. In the sample data collection step at 101, inertial sensor data is collected over a capture time interval [t _i , t _f ] having a start time 203 (t _i ) and an end time 204 (t _f ). In one embodiment, the inertial sensor includes a 3-axis accelerometer and a 3-axis rate gyro. In other embodiments, different data may be available including, for example, data from a magnetometer or location data from a GPS receiver. In one embodiment having a 3-axis accelerometer and a 3-axis rate gyro, sample data captured from the sensor is:

Time-series accelerometer data 201 written as:

And time-series rate gyro data 202 expressed as:

as well as

The boldface notation indicates that these are vector quantities (usually in three-dimensional space), but for purposes of illustration, these values are assumed to be one-dimensional in curves 201 and 202. It is shown. Accelerometer data notation

Reflects the fact that normal accelerometers measure specific forces, not true accelerations. Notation of rate gyro data

Indicates that a normal rate gyro measures the angular velocity.

In FIG. 2, the initialization interval 205 is selected within the capture interval [t _i , t _f ]. The initialization interval [t ₀ , t _n ] ⊂ [t _i , t _f ] has a start point 206 (t ₀ ) and an end point 207 (t _n ). The initialization interval can be selected within the capture interval based on various criteria depending on the characteristics of the event of interest. For example, in one or more embodiments, the initialization interval may consist of a fixed number of seconds at the start of the capture interval. In other embodiments, the initialization interval may be determined dynamically by searching the capture interval for signatures that signal relatively sluggish movement or activity. Once the initialization interval 205 is selected, the accelerometer data 208 and gyro data 209 over the initialization interval are used for initial orientation and initial velocity estimation described below.

  FIG. 3 shows a detailed view of step 103 for defining soft constraints and step 104 for defining penalty metrics.

Inertial sensor position trajectory 301 and

It is expected that the inertial sensor velocity trajectory 305, denoted as ## EQU1 ## approximately conforms to a particular pattern typical of the subject's activity. In general, the position and velocity of the inertial sensor are vector quantities in a three-dimensional space, but for purposes of illustration, these are shown as one-dimensional values in FIG. The particular pattern expected for position and velocity will depend on the particular activity or activities of the subject. In general, soft constraints C _{i on} position and velocity are a subset of possible trajectories of position and velocity over the initialization interval.

Can be expressed as An embodiment of the present invention may include a set of these soft constraints {C _i }. FIG. 3 illustrates exemplary constraints 302 and 306. These are only examples for purposes of illustration, and other embodiments will include other types and numbers of soft constraints. Soft constraints 302, labeled C _R is a constraint on the position trajectory 301. This constraint is

Is required to stay within the upper limit 303 and the lower limit 304 throughout the time interval of interest. A soft constraint 306 written as _CV is a constraint on the velocity trajectory 305. The constraint 306 applies only at the end of the trajectory, which requires the speed to be within the limits defined by _CV . These exemplary soft constraints 302 and 306 illustrate simple examples of the types of possible soft constraints, and other embodiments may include more complex constraints on position, velocity, or both. Good.

  In 104 of FIG. 3, a penalty metric is defined to quantify the deviation of the position and velocity trajectory from the soft constraints defined in 103.

The measured or calculated position 307 written as “does not satisfy the soft constraint 302. penalty measurement reference 308, labeled m _R is from soft constraint

Measure deviations. For illustrative purposes, the penalty metric m _R is defined as the maximum absolute deviation of the position from the limit imposed by the soft constraints. Other embodiments may define different penalty metrics, for example, one embodiment uses the average absolute deviation of the position from the soft constraint limit rather than the maximum deviation shown in FIG. It will be possible. The speed penalty metric diagram is similar to the position penalty metric diagram.

The measured or calculated speed 309 written as “does not satisfy the soft constraint 306. penalty measurement reference 310, labeled m _V is from soft constraint

Measure deviations. For illustrative purposes, the penalty measurement reference m _V is defined as the absolute deviation of the velocity from the limits imposed by the software constraints. Since the exemplary speed soft constraint 306 applies only at the end of the time interval, the speed penalty metric 310 similarly measures the deviation from the constraint only at this end point. Similar to the position penalty measurement standard, this speed penalty measurement standard is only an example. Other embodiments may define different penalty metrics, for example, one embodiment is not the absolute value of the deviation shown in FIG. 3 but the square of the absolute deviation of the velocity from the soft constraint limit. Could be used.

  FIG. 4 shows a detailed view of the steps for calculating the initial orientation and initial velocity at 105 to minimize the penalty measurement criteria.

Accelerometer data 208 and

The rate gyro data 209 written as is an input to the calculation procedure. These trajectories are available over the initialization interval [t ₀ , t _n ]. Given these inputs, the sensor orientation (Q ₀ ) and speed

When the initial state 402 is given, the position locus 307

And velocity trajectory 309

To obtain the differential equation 401. The differential equation 401 has a direction Q (t), speed

And position

Observed measurement of time derivative of

as well as

Is a set of simultaneous expressions related to. These differential equations are well known to those skilled in the art, and are a form of equation for inertial navigation using strapdown inertial sensors. Notation in 401

Is

Shows the linear operator S defined by: where x is the cross product of the vectors.

  In the calculation of 105, the initial state 402 is treated as an unknown quantity to be estimated. Each estimate of these initial states yields a specific pair of position and velocity trajectories 307 and 309. Penalty metrics 308 and 310 are then applied to these calculated trajectories. These penalty metrics generate a collective measure of the deviation of the calculated trajectory from the soft constraints. Different embodiments of the present invention provide a set of penalty metrics, such as adding a penalty metric, or minimizing one metric first and then minimizing the other metric, to a set of soft constraint deviations. Various techniques may be used to combine to a common scale. At 403, the initial state is adjusted to minimize the observed penalty metrics 308 and 310. This adjustment process may be iterative as shown in FIG. 4 to determine an initial state that minimizes the penalty metric. Any known technique for minimizing the objective function may be used to find an initial state that minimizes the penalty metric. Such techniques are well known to those skilled in the art. After applying the selected minimization technique, the calculation at 105 yields an initial orientation estimate

And for initial speed estimates

Is an estimate of the initial state 404 labeled.

  FIG. 5 illustrates one embodiment of the present invention with specific soft constraints for the position trajectory that reflects the expected minimum motion of the inertial sensor during the initialization interval. This type of soft constraint could be applied, for example, to an activity where a person or device is relatively static before the start of the activity. FIG. 5 illustrates the manner in which the procedure of FIG. 4 is modified and specialized for this type of embodiment. Similar to FIG. 4, the accelerometer data 208 and the gyro data 209 are input to the integral of the differential equation 401 together with the initial state 402, thereby obtaining a position locus 307. In the embodiment shown in FIG. 5, the position penalty metric 502 is the magnitude of the net change in position across the initialization interval.

Is defined as This penalty metric measures how close the position trajectory is during the initialization interval and conforms to the soft constraint of zero sensor net movement. Other embodiments may use different penalty metrics, such as maximum change in position over the initialization interval from the starting position, to measure this deviation. In the embodiment shown in FIG. 5, the initial speed estimate is

By setting to, further simplification 501 is applied to the initial state. This simplification again reflects the expectation of relatively little movement during the initialization interval. Other embodiments may utilize different assumptions regarding the initial speed. Accordingly, the initialization step 503 includes the position penalty metric 502.

To minimize,

Only the initial orientation Q ₀ is adjusted to yield an initial orientation estimate 504 labeled.

  FIG. 6 illustrates a technique that can be applied in one or more embodiments of the present invention to integrate the differential equation 401 by using the initial orientation and initial velocity inputs 402 and sensor data 208 and 209. Yes. In this technique, the differential equation 601 is first solved separately. This formula

Is solved at 603 by defining an auxiliary variable P, where Q = Q ₀ P. This substitution is a formula with the initial condition P (0) = I

Where I is the identity matrix. This equation can be solved directly for P (t) without any dependence on the initial orientation Q ₀ (in most cases, P (t) is calculated by numerical integration. And the reason is that there is a high probability that no closed-form solution exists). Second, the remaining differential equation 602 can be solved by using the value of P (t) calculated at 603 as the input 604. These two equations are integrated at 605, resulting in the following position and velocity equations:

as well as

In one or more embodiments, these equations are used to calculate an initial state 402 that minimizes the penalty metric.

  FIG. 7 shows a specific position penalty metric 502.

And a technique used in one or more embodiments to directly calculate the initial orientation Q ₀ that minimizes the penalty metric using the equation of 605.

By using the simplification 501 and the auxiliary variable at 701

By defining

Where

It is.
This expression is an initial value

Together with the diagram at 702. Q ₀ is an orthogonal transformation, so possible values

Range of radius

Must be located on a sphere with A two-dimensional view of this sphere is shown as circle 703.

From

Rotation to is shown at 704, and in all cases,

Is located on 703. Therefore,

In order to minimize

But,

, So that at 705 the optimal value 706 and the minimum

It is geometrically clear that it should lead to This technique is

Optimal up to any rotation around

This reflects that the pure inertial sensor cannot detect the orientation. In other embodiments of the invention, other sensor data, such as magnetometer data, may be used as well to estimate the initial orientation.

  FIG. 8 shows a flowchart in one embodiment of the present invention using a soft constraint that requires the inertial sensor motion to be approximately periodic during the initialization interval. Soft constraint definition 103 includes defining 805 periodic constraints for inertial sensor velocity and position during the initialization interval. In a state corresponding to the periodicity soft constraint, the penalty metric definition 104 includes a metric that measures a change in position and velocity over a single period at 806 as a penalty metric. At 102, an initialization interval is selected, and at 801, an estimate of the period of motion during this interval is calculated. This period calculation is performed by converting the time-series sensor gyro data to the frequency domain via a discrete Fourier transform at 802 and selecting a dominant frequency from the converted data at 803. At 804, a particular time interval within an initialization interval of length equal to the period is selected. Penalty measurement standard 806 first calculates an initial orientation estimate to minimize the speed penalty measurement standard at 807, and then calculates initial speed estimate to minimize the position penalty measurement standard at 808. It is minimized at 105 in two stages. Other embodiments of the invention may utilize a subset of these techniques, for example, the same velocity and position penalty metric may be used, but in order to estimate the period of motion, a Fourier transform A time domain technique could be used instead of.

  FIG. 9 provides a detailed view of the periodicity soft constraint definition step 805 and the position and velocity penalty metric definition step 806. At 805, the soft constraint is (

Position 901 and (

Need to have a periodicity with some period 903 (denoted as T). These soft constraints only require periodic movement and do not define the period or shape of the position and velocity curves. At 806, the calculated position 904 and the calculated velocity 906 do not have accurate periodicity. The position penalty metric 905 is the absolute value of the change in position over one period.

Measure the deviation from the exact periodicity. Similarly, the speed penalty metric 907 is also the absolute value of the change in speed over one period.

Measure the deviation from the exact periodicity.

FIG. 10 provides a detailed view of the motion period calculation step 801. Time domain rate gyro data during an initialization interval including time series 1001 of ω _{x on} the x-axis of the gyro and similar time series of ω _y and ω _z

Has been transformed to the frequency domain using a discrete Fourier transform. The transformed ω _x magnitude 1002 labeled | Ω _x | is scanned for its peak value. A peak value 1003 occurs at frequency 1004. Similar scanning of magnitude | Ω _y | and | Ω _z | has been performed to find the peak values on these axes and the frequencies at which these peak values occur. At 1005, the maximum peak size across the three axes has been selected (shown on the z-axis for illustrative purposes), and the frequency 1006, denoted f _max , at which this peak occurs is It is used as the overall dominant frequency of movement during initialization. Next, at 1007, the dominant period is calculated as T = 1 / f _max .

FIG. 11 provides a detailed view of the initial orientation estimate calculation step at 807 and the initial velocity estimate calculation step at 808. An interval 1101 of length T found at 804 labeled [t ₁ , t ₁ + T] is used for the calculation. Penalty measurement standards 905 and 907 are minimized in order. By using the technique described above and shown in FIG. 6, equation 1102 is obtained, which results in the following equation for the velocity penalty metric:

Is obtained. By minimizing this expression by using the geometric technique shown in FIG. 7 or by using any other well-known technique for numerical optimization, Estimated value

Can be obtained. The initial estimate 1103 is used in subsequent calculations at 808. Again, to obtain Equation 1104, the integration technique shown in FIG. 6 is used, which results in

Is obtained. Where this equation is a single unknown

Depends only on the reason,

This is because is calculated in advance. Here, this can be achieved by using any well-known technique for numerical optimization.

Estimated initial speed by minimizing

Can be obtained.

  Hereinafter, preferable embodiments of the present invention will be described in terms of items.

Embodiment 1
Inertial sensor initial orientation Q ₀ and initial speed

In a method for determining an estimate of
Collecting sample data from the inertial sensor over a capture time interval [t _i , t _f ] that includes the activity of interest, the sample data comprising acceleration data

And rate gyro data

Including steps, and
Selecting an initialization interval [t ₀ , t _n ] ⊂ [t _i , t _f ] within the capture time interval;
Position of the inertial sensor during the initialization interval

And speed

Defining soft constraints {C _i } for

The soft constraint represents an approximation of a representative pattern of the position and velocity trajectory of the subject's activity during the initialization interval; and
The position from the soft constraint {C _i }

And the speed

Penalty metrics for quantifying deviations in calculated trajectories

A step of defining
The initial orientation and initial velocity are combined differential equations

Via the initialization interval to minimize the penalty metric when used as an initial state to integrate the sample data into orientation, position, and velocity over the initialization interval. The initial orientation Q ₀ = Q (t ₀ ) and initial velocity of the inertial sensor at

Calculating an estimated value of the method.

Embodiment 2
Among the soft constraints, the change in position between the start and end of the initialization interval is zero, i.e.

Including a requirement to be
Set the initial speed estimate to zero, i.e.

Step to set to
Include in the penalty metric the absolute value of the position difference between the end of the initialization interval and the start of the initialization interval, i.e.

The method of embodiment 1, further comprising the step of:

Embodiment 3
Including, among soft constraints, a requirement that the position and velocity of the inertial sensor have periodicity during the initialization interval;
By applying a discrete Fourier transform to the corresponding axis of the time series of rate gyro data during the initialization interval, a frequency domain signal Ω _x (f) = DFT (ω _x for each axis of the rate gyro. (T)), Ω _y (f) = DFT (ω _y (t)), Ω _z (f) = DFT (ω _z (t));
The frequency f _max = argmax (max (| Ω _x (f) |, | Ω _y (f) |, | Ω _z (f) |) having the maximum transformation magnitude across the three axes with the dominant frequency f _max ) And calculating the dominant period T from the dominant frequency as T = 1 / f _max ;
Among the penalty measurement standards, a speed penalty measurement standard that is an absolute value of the change in speed over one period of movement

And a position penalty metric that is the absolute value of the change in position over one period of motion

Including a step, and
For the calculation of the initial velocity and the initial direction estimate, the period specified in the initialization interval is selected, ie, [t ₁ , t ₁ + T] ⊂ [t ₀ , t _n ]. When,
Calculating the initial orientation estimate by minimizing the speed penalty metric over the specified period;
Calculating the initial velocity estimate by minimizing the position penalty metric over the specified time period using the initial orientation estimate. The method according to aspect 1.

Claims (3)

Inertial sensor initial orientation Q ₀ and initial speed

In a method for determining an estimate of
Collecting sample data from the inertial sensor over a capture time interval [t _i , t _f ] that includes the activity of interest, the sample data comprising acceleration data

And rate gyro data

Including steps, and
Selecting an initialization interval [t ₀ , t _n ] ⊂ [t _i , t _f ] within the capture time interval;
Position of the inertial sensor during the initialization interval

And speed

Defining soft constraints {C _i } for

The soft constraint represents an approximation of a representative pattern of the position and velocity trajectory of the subject's activity during the initialization interval; and
The position from the soft constraint {C _i }

And the speed

Penalty metrics for quantifying deviations in calculated trajectories

A step of defining
The initial orientation and initial velocity are combined differential equations

Via the initialization interval to minimize the penalty metric when used as an initial state to integrate the sample data into orientation, position, and velocity over the initialization interval. The initial orientation Q ₀ = Q (t ₀ ) and initial velocity of the inertial sensor at

Calculating an estimated value of the method. Among the soft constraints, the change in position between the start and end of the initialization interval is zero, i.e.

Including a requirement to be
Set the initial speed estimate to zero, i.e.

Step to set to
Include in the penalty metric the absolute value of the position difference between the end of the initialization interval and the start of the initialization interval, i.e.

The method of claim 1, further comprising the step of: Including, among soft constraints, a requirement that the position and velocity of the inertial sensor have periodicity during the initialization interval;
By applying a discrete Fourier transform to the corresponding axis of the time series of rate gyro data during the initialization interval, a frequency domain signal Ω _x (f) = DFT (ω _x for each axis of the rate gyro. (T)), Ω _y (f) = DFT (ω _y (t)), Ω _z (f) = DFT (ω _z (t));
The frequency f _max = argmax (max (| Ω _x (f) |, | Ω _y (f) |, | Ω _z (f) |) having the maximum transformation magnitude across the three axes with the dominant frequency f _max ) And calculating the dominant period T from the dominant frequency as T = 1 / f _max ;
Among the penalty measurement standards, a speed penalty measurement standard that is an absolute value of the change in speed over one period of movement

And a position penalty metric that is the absolute value of the change in position over one period of motion

Including a step, and
For the calculation of the initial velocity and the initial direction estimate, the period specified in the initialization interval is selected, ie, [t ₁ , t ₁ + T] ⊂ [t ₀ , t _n ]. When,
Calculating the initial orientation estimate by minimizing the speed penalty metric over the specified period;
Calculating the initial velocity estimate by minimizing the position penalty metric over the specified time period using the initial orientation estimate. Item 2. The method according to Item 1.

Images