JP2013108865A - Inertial navigation operation method and inertia navigation operation device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a new method of inertial navigation operation which considers moving situations of a user.SOLUTION: Moving situations of a user are determined by a moving situation determination part 20. By a parameter value setting part 40, value of a parameter related to prescribed error estimation processing for estimating errors included in operation value related to the inertial navigation operation performed by an INS 10 is set based on the determination result of the moving situations. An error estimation processing part 50 performs error estimation processing by using set parameter value, and the INS 10 corrects the operation value by using the error estimated by the error estimation processing, and performs the inertial navigation operation.

Description

  The present invention relates to a method for performing inertial navigation calculation.

  In various fields such as so-called seamless positioning, motion sensing, and attitude control, the use of sensors is drawing attention. As sensors, acceleration sensors, gyro sensors, pressure sensors, geomagnetic sensors, and the like are widely known. A technique for calculating the position of a moving body (for example, a bicycle, a car, a train, a ship, an airplane, etc.) by performing inertial navigation calculation using the measurement result of the sensor has been devised.

  Among them, Patent Literature 1 and Patent Literature 2 disclose techniques for calculating the position of a person using inertial navigation or the like.

US Pat. No. 7,245,254 US Patent Publication No. 2010/0198511

  In the position calculation technique for walking, the accuracy of position calculation depends solely on the measurement error of the inertial sensor mounted on the position calculation device. Therefore, it is necessary to correct the measurement error of the inertial sensor. The user does not always move by walking, and the movement situation of the user always changes. For example, assuming a situation in which a position calculation device is attached to a part of the user's body and the user's position is obtained by inertial navigation calculation on a daily basis, the user can move on a bicycle in addition to walking. When the user moves on a car or moves on an elevator, the movement status of the user changes from time to time.

  It is considered that the measurement error of the inertial sensor changes according to such a movement situation of the user. That is, during walking, the measurement error of the inertial sensor tends to increase due to the user's foot movement and vertical movement of the body. However, the measurement error of the inertial sensor tends to be small if the user is standing still or riding a moving object that does not require physical exercise.

  The present invention has been made in view of the above-described problems, and an object of the present invention is to propose a new inertial navigation calculation method in consideration of a user's movement status.

  In the first embodiment for solving the above-described problems, either the user's movement status is determined, or a calculation result or a calculation process value related to a predetermined inertial navigation calculation (hereinafter referred to as “calculation value”). Setting a parameter value related to a predetermined error estimation process for estimating an error included in () based on the determination result of the movement status; and executing the error estimation process using the parameter value; And performing the inertial navigation calculation by correcting the calculated value using the error estimated by the error estimation process.

  As another form, a determination unit that determines a user's movement status, and a parameter value related to a predetermined error estimation process that estimates an error included in a calculation value related to a predetermined inertial navigation calculation, A setting unit configured to set based on the determination result; an error estimation processing unit that executes the error estimation process using the parameter value; and the calculation value is corrected using the error estimated by the error estimation process. An inertial navigation calculation device including an inertial navigation calculation unit that performs the inertial navigation calculation may be configured.

  According to the first aspect and the like, the user's movement situation is determined, and the parameter value related to the predetermined error estimation process for estimating the error included in the calculation value related to the predetermined inertial navigation calculation is determined. Set based on the results. Thereby, based on a user's movement condition, the value of the parameter which concerns on an error estimation process is optimized, and the error contained in a calculation value can be estimated correctly. As a result, a new inertial navigation calculation method capable of calculating a more accurate position can be realized.

  Further, as a second mode, in the inertial navigation calculation method according to the first mode, the inertial navigation calculation is a navigation calculation using a measured value of the acceleration of the user, and the error estimation process includes the parameter An inertial navigation calculation method, which is a Kalman filter process using a parameter based on an error of a measurement value, may be configured.

  According to the second embodiment, the inertial navigation calculation is a navigation calculation using a measurement value of the user's acceleration, but the measurement value of the acceleration may include an error. Therefore, a parameter set based on the determination result of the movement state is a parameter based on an error in the acceleration measurement value. Then, by executing the Kalman filter process using the parameter based on the error of the measurement value, it is possible to correctly estimate the error included in the calculated value.

  Further, as a third form, in the inertial navigation calculation method of the second form, the parameter includes a parameter relating to each axis defining a predetermined spatial coordinate system, and the setting is performed in the direction of gravity. An inertial navigation calculation method including setting the value of the parameter based on the fact that the error variance is larger than the error variance in the other direction may be configured.

  For example, when the user walks, the body moves up and down, so that the error included in the measured value of acceleration in the vertical direction tends to increase and the variance tends to increase. Therefore, as in the third embodiment, parameter values are set based on the fact that the variance of errors in the gravitational direction is greater than the variance of errors in other directions in a predetermined spatial coordinate system. This makes it possible to more accurately estimate the error included in the calculated value.

  In addition, as a fourth mode, in the inertial navigation calculation method according to the third mode, the determination of the moving state includes determining whether it is at least a walking state or a stationary state, and the setting is performed. An inertial navigation calculation including setting the value of the parameter based on the fact that the variance of the error in the gravitational direction is larger when the determination result of the moving situation is a walking situation than in a stationary situation A method may be configured.

  In the situation where the user is walking, the error in the direction of gravity is larger and the variance tends to be larger than in the situation where the user is stationary. Therefore, as in the fourth embodiment, it is determined whether the situation is at least a walking situation or a stationary situation. Then, by setting the parameter value based on the fact that the variance of the error in the gravitational direction is larger than that in the stationary situation when the judgment result of the moving situation is the walking situation, the error included in the calculated value Can be estimated more accurately.

  Further, as a fifth aspect, in the inertial navigation calculation method according to any one of the second to fourth aspects, the determination of the movement state includes at least determining whether the movement state is present, and the movement state If the determination result is a walking situation, detecting the user's stride and one-step required time based on a measured value of acceleration in the vertical direction of the user, the stride and one-step required time, and the movement of the user And calculating the speed vector of the user during walking based on the direction, and the error estimation process sets the parameter to the measured value when the determination result of the movement state is a walking state. An inertial navigation calculation method, which is a Kalman filter process using the velocity vector as an observation value as a parameter based on an error, may be configured.

  According to the fifth aspect, it is determined whether or not the user is in a walking situation, and when the user is in a walking situation, the user's stride and the time required for one step are detected based on the measured value of acceleration in the vertical direction of the user. There is a correlation between the user's vertical acceleration and stride. Further, the time required for one step can be determined from the peak value of the time-series change in the vertical acceleration. By using the stride and the time required for one step and the moving direction of the user, the speed vector of the user during walking can be calculated. Then, when the determination result of the moving situation is a walking situation, the parameter is a parameter based on the error of the acceleration measurement value, and the calculated value is included in the calculated value by performing the Kalman filter processing using the calculated velocity vector as an observation value. The error can be estimated appropriately.

  Further, as a sixth aspect, in the inertial navigation calculation method according to any one of the second to fifth aspects, the determination of the movement state determines at least whether or not the user is on a moving body. The error estimation processing includes determining the parameter as a parameter based on the error of the measurement value and determining a predetermined constraint velocity vector as an observed value when the determination result of the movement condition is a condition of riding on a moving object. It is also possible to constitute an inertial navigation calculation method that is Kalman filter processing.

  According to the sixth aspect, it is determined whether or not at least the user is on the moving body. Then, when the determination result of the moving state is a state of riding on the moving body, the Kalman filter process is performed using the parameter as a parameter based on the error of the measurement value and the predetermined restricted velocity vector as the observation value. If the user is on the moving body, the moving direction of the user matches the moving direction of the moving body. Therefore, the error included in the calculated value can be appropriately estimated by setting the restricted speed vector based on the moving direction of the moving body.

Explanatory drawing of the system configuration | structure of the whole system. 1 is an explanatory diagram of a system configuration of INS. FIG. The system block diagram of an INS calculation system. The block diagram which shows an example of a function structure of an inertial navigation calculating apparatus. The figure which shows an example of the data structure of the data for input error covariance matrix setting. The figure which shows an example of a data structure of step length model data. The flowchart which shows the flow of a main process.

  Hereinafter, an example of a preferred embodiment of the present invention will be described with reference to the drawings. However, it goes without saying that embodiments to which the present invention can be applied are not limited to the embodiments described below.

1. Principle 1-1. Overall System FIG. 1 is a diagram showing a schematic configuration of an overall system 1 in the present embodiment. The overall system 1 includes an INS (Inertial Navigation System) 10, a movement status determination unit 20, an observation information generation unit 30, a parameter value setting unit 40, and an error estimation processing unit 50. However, these functional units are described as examples, and all of these functional units do not necessarily have to be essential components.

  In the drawings to be referred to below, a sensor block is indicated by a double solid line, and a processing block that performs arithmetic processing using the sensor measurement result is indicated by a single solid line. For example, a processor (host processor) mounted on an electronic device is a processing entity of the processing block. The subject of processing of each processing block can be appropriately set according to the system to which the present invention is applied.

  In this embodiment, two types of coordinate systems are defined. The first coordinate system is a local coordinate system (sensor coordinate system) composed of a three-dimensional orthogonal coordinate system associated with a sensor. In the present embodiment, the three axes of the local coordinate system are denoted as x-axis, y-axis, and z-axis.

  The second coordinate system is an absolute coordinate system that is a spatial coordinate system used as a reference when performing inertial navigation calculation. The absolute coordinate system is, for example, an NED (North East Down) coordinate system known as a northeast lower coordinate system, and is a coordinate system including an axis in the gravity direction. In the present embodiment, the three axes of the absolute coordinate system are expressed as an X axis, a Y axis, and a Z axis.

  In addition to magnitude, acceleration and speed have directions. In this specification, in principle, acceleration and velocity refer to the magnitude of acceleration and velocity (scalar amount), and acceleration and velocity vectors refer to magnitude (scalar amount). It shall represent the direction. In addition, in order to clarify various quantities defined in each coordinate system, a description will be given with the type of the coordinate system added to the head of the word representing each quantity. For example, an acceleration vector expressed in the local coordinate system is referred to as “local coordinate acceleration vector”, and an acceleration vector expressed in the absolute coordinate system is referred to as “absolute coordinate acceleration vector”. The same applies to other quantities.

  The INS 10 is known as an inertial navigation system, and is a system configured to be capable of autonomous positioning. The INS 10 is based on measurement results measured by an inertial sensor such as an acceleration sensor 5A and a gyro sensor 5B, or an IMU (Inertial Measurement Unit) in which these inertial sensors are packaged. Is calculated and output. The IMU is a sensor unit known as an inertial measurement unit, and is configured to be able to measure and output an acceleration vector and an angular velocity expressed in a local coordinate system.

  FIG. 2 is a diagram illustrating an example of a system configuration of the INS 10. The INS 10 includes an acceleration sensor 5A and a gyro sensor 5B as inertial sensors. As processing blocks, an attitude information calculation unit 11, an absolute coordinate acceleration vector calculation unit 13, an absolute coordinate velocity vector calculation unit 15, an absolute coordinate position calculation unit 17, a gravity calculation unit 18, and a correction unit 19 are provided. Have. The INS 10 is an inertial navigation calculation unit that performs an inertial navigation calculation by correcting the calculation value using the error estimated by the error estimation processing unit 50.

  The acceleration sensor 5A is a sensor that measures acceleration in a local coordinate system. The gyro sensor 5B is a sensor that measures the angular velocity in the local coordinate system. For these sensors, for example, MEMS sensors using the technology of MEMS (Micro Electro Mechanical Systems) are used.

  The posture information calculation unit 11 calculates the latest posture information using the angular velocity measured by the gyro sensor 5B and the posture information stored in the posture information storage unit 11A. The posture information means information indicating postures defined based on various posture expressions such as quaternions, direction cosine matrices (DCM), and Euler angles. The posture information calculation unit 11 stores the calculated latest posture information in the posture information storage unit 11A.

  The absolute coordinate acceleration vector calculation unit 13 calculates the latest absolute coordinate acceleration vector using the local coordinate acceleration vector measured by the acceleration sensor 5A and the posture information input from the posture information calculation unit 11. Specifically, the acceleration vector in the local coordinate system is coordinate-converted to the absolute coordinate system, and the latest absolute coordinate acceleration vector is calculated. A known technique can be applied to the coordinate conversion, and thus description thereof is omitted. The absolute coordinate acceleration vector calculation unit 13 stores the calculated latest absolute coordinate acceleration vector in the absolute coordinate acceleration vector storage unit 13A.

  The absolute coordinate velocity vector calculation unit 15 includes the absolute coordinate acceleration vector input from the absolute coordinate acceleration vector calculation unit 13, the gravity information input from the gravity calculation unit 18, and the absolute coordinate velocity stored in the absolute coordinate velocity vector storage unit 15A. An absolute coordinate velocity vector is calculated using the vector. At this time, the gravity direction and the Coriolis force are corrected based on the gravity information. Since the earth is elliptical, the latitude seen from the center of the earth is different from the geographical latitude. Therefore, it is necessary to correct the direction of gravity, and since the earth is rotating with respect to the inertial space of the universe, it is also necessary to correct the Coriolis force.

  The absolute coordinate velocity vector calculation unit 15 adds the relative velocity vector obtained by integrating the absolute coordinate acceleration vector in consideration of the direction of gravity as described above, and stores the absolute coordinate velocity stored in the absolute coordinate velocity vector storage unit 15A. The latest absolute coordinate velocity vector is calculated by adding to the vector. Then, the latest calculated absolute coordinate velocity vector storage unit 15A is stored.

  The absolute coordinate position calculation unit 17 adds the relative position vector obtained by integrating the absolute coordinate velocity vector input from the absolute coordinate velocity vector calculation unit 15 to the absolute coordinate position stored in the absolute coordinate position storage unit 17A. Thus, the absolute coordinate position is calculated. Then, the calculated latest absolute position coordinates are stored in the absolute coordinate position storage unit 17A.

  The gravity calculation unit 18 calculates gravity information using the absolute coordinate position input from the absolute coordinate position calculation unit 17 and outputs the gravity information to the absolute coordinate velocity vector calculation unit 15.

  The correction unit 19 corrects the integrated value calculated by each calculation unit using the error information estimated by the error estimation processing unit 50. Specifically, the posture information stored in the posture information storage unit 11A is corrected using the posture angle error included in the error information. Further, the angular velocity measurement value from the gyro sensor 5B and the acceleration measurement value from the acceleration sensor 5A are respectively corrected using the angular velocity bias and the acceleration bias included in the error information.

  Further, the absolute coordinate velocity vector stored in the absolute coordinate velocity vector storage unit 15A is corrected using the velocity vector error included in the error information, and the absolute coordinate position stored in the absolute coordinate position storage unit 17A is converted into error information. Is corrected using the position error included in.

  INS 10 finally outputs calculation values of posture information, absolute coordinate velocity vector, absolute coordinate position, acceleration vector, and angular velocity. Among these calculated values, the absolute coordinate position is an example of a value of a calculation result related to a predetermined inertial navigation calculation. The other quantities are examples of values of a calculation process related to a predetermined inertial navigation calculation.

  Although not shown in FIG. 2, the INS 10 includes an error included in the acceleration measurement value from the acceleration sensor 5A (hereinafter referred to as “acceleration error”) and an angular velocity measurement value from the gyro sensor 5B. The included error (hereinafter referred to as “angular velocity error”) is calculated, and the calculated value is output to the error estimation processing unit 50.

  The movement status determination unit 20 determines the movement status of the user. Specifically, the movement status determination unit 20 is based on information such as an INS calculation result (posture information, velocity vector, position, acceleration, angular velocity) calculated by, for example, the INS 10 (hereinafter referred to as a user walking). , “Walking situation”), stationary situation (hereinafter referred to as “stationary situation”), or situation where the user gets on the moving body (hereinafter referred to as “ride situation”). .). As the moving body, it is possible to assume a moving body used by the user on a daily basis, such as a four-wheeled vehicle, a two-wheeled vehicle, a train, a bicycle, and an elevator.

  In a stationary situation, in principle, values such as an acceleration vector, an angular velocity, and a velocity vector are zero, so that the stationary situation can be determined by detecting this state. In a walking situation, for example, the vertical acceleration vibrates at a very short period. Therefore, the walking situation can be determined by detecting this state. In a riding situation, if the user is stationary, in principle, acceleration is detected only in the traveling direction of the moving body. Therefore, for example, it is determined whether or not the user is on the moving body by determining whether or not accelerations of substantially the same magnitude are detected for the same axial direction for a certain time or more. Can be judged.

  Returning to FIG. 1, the observation information generation unit 30 generates observation information used when the error estimation processing unit 50 performs the error estimation processing based on the determination result of the movement status determination unit 20.

  Specifically, when the user's movement state is “walking state”, the stride and the time required for one step (hereinafter referred to as “one step required time”) are detected based on the vertical acceleration of the user. To do. Then, based on the stride, the time required for one step, and the moving direction of the user, a speed vector when the user is walking (hereinafter referred to as a “speed vector during walking”) is calculated and used as observation information. The direction of movement of the user may be measured, for example, by installing an orientation sensor such as a geomagnetic sensor in the device, or a GPS sensor using a GPS (Global Positioning System) which is a kind of satellite positioning system. It may be measured by installing it in

  If the user's movement status is “stationary”, the user's speed should ideally be zero (the velocity vector is zero vector). Therefore, for example, “the velocity component of each axis of the user in the absolute coordinate system = 0” can be given as the observation information.

  When the movement status of the user is “boarding status”, the traveling direction of the moving body on which the user is riding is detected, and a predetermined restricted velocity vector is used as observation information. In the present embodiment, a four-wheeled vehicle is assumed as the moving body. Normally, four-wheeled vehicles do not jump or skid, so it is assumed that the vertical and horizontal speed components of a four-wheeled vehicle are zero, and the “velocity component in the vertical and horizontal directions of the moving object = 0” is given as observation information. .

  Of course, the moving body may be assumed to be other than a four-wheeled vehicle. For example, when an elevator is considered, since the elevator does not move in any direction other than the vertical direction, “velocity component in the longitudinal direction of the moving body = 0” can be given as observation information.

  The parameter value setting unit 40 sets a parameter value related to a predetermined error estimation process for estimating an error included in a calculated value related to the inertial navigation calculation performed by the INS 10. The method for setting this parameter value will be described later in detail.

  The error estimation processing unit 50 uses the observation information generated by the observation information generation unit 30 to perform a predetermined error estimation process for estimating an error included in the calculated value calculated by the INS 10. As the error estimation processing, in the present embodiment, a case where Kalman filter processing is applied will be described as an example.

  Specifically, attitude information error (attitude error) calculated by INS10, absolute coordinate velocity vector error (velocity vector error), absolute coordinate position error (position error), acceleration sensor 5A bias (acceleration bias) And a state vector “x” having a component of the bias (gyro bias) of the gyro sensor 5B as a component. Further, an acceleration error and an angular velocity error output as calculation values from the INS 10 are set as an input error vector “u”. Further, the observation information generated by the observation information generation unit 30 is set as an observation vector “z”.

  As described above, the state vector “x”, the input error vector “u”, and the observation vector “z” are set, the prediction calculation (time update) and the correction calculation (observation update) based on the Kalman filter theory are performed, and the state An estimated value (state estimated value) of the vector “x” is obtained. Each component of the state estimated value becomes an error included in the calculated value related to the inertial navigation calculation.

  The error information estimated by the error estimation processing unit 50 is fed back to the INS 10. As described above, based on the fed back error information, the integration executed in each processing block of the INS 10 is corrected by the correction unit 19 using the error information. That is, the calculated value is corrected using the error estimated by the error estimation process, and the inertial navigation calculation is performed.

1-2. Setting of Parameter Value In this embodiment, the state equation in Kalman filter processing is given by the following equation (1).

  Here, “x” is a state vector, “u” is an input error vector, and “v” is a process error vector. “F”, “A”, and “B” are matrices for converting each vector into a state space.

  In the present embodiment, the input error vector “u” is a total of six components of the three-dimensional acceleration (acceleration measurement value) measured by the acceleration sensor 5A and the three-dimensional angular velocity (angular velocity measurement value) measured by the gyro sensor 5B. Let each error be. When the user walks, inertial sensors such as the acceleration sensor 5A and the gyro sensor 5B are constantly subjected to vibrations and shocks by walking motion. For this reason, the output values from these inertial sensors do not necessarily indicate the true acceleration or angular velocity of the user, and may include large errors. Also, these errors tend to vary greatly.

Therefore, consider the covariance of each component of the input error vector “u”. At this time, the covariance of each component of the input error vector “u” can be represented by a diagonal matrix with the angular velocity and acceleration error variance values as diagonal components. Specifically, as a matrix representing the covariance of the input error, a covariance matrix “Qu” of the input error vector “u” as shown in the following equation (2) (hereinafter referred to as “input error covariance matrix”): Can be defined.

In equation (2), the square values of “σ _gx ”, “σ _gy ”, and “σ _gz ” are variance values of errors in angular velocities around the x-axis, y-axis, and z-axis of the local coordinate system. In addition, the square values of “σ _ax ”, “σ _ay ”, and “σ _az ” are variance values of the error of acceleration on the x-axis, y-axis, and z-axis of the local coordinate system, respectively.

In Expression (1), “v” is a process error vector whose component is a process error included in angular velocity and acceleration, and is noise inherent in the system. The covariance of each component of the process error vector “v” can be represented by a diagonal matrix with the angular velocity and acceleration bias variance values as diagonal components. Specifically, a covariance matrix “Qv” of the process error vector “v” as shown in the following equation (3) can be defined.

In Equation (3), the square values of “σ _gbx ”, “σ _gby ”, and “σ _gbz ” are dispersion values of the bias of the angular velocity around the x-axis, y-axis, and z-axis of the local coordinate system. Also, the square values of “σ _abx ”, “σ _aby ”, and “σ _abz ” are variance values of the bias of acceleration on the x-axis, y-axis, and z-axis of the local coordinate system.

At this time, when the state equation of equation (1) is rewritten as a state equation of a discrete time system, the following equation (4) is obtained.

However, the subscript “k” indicates the k-th calculation step. Also, “φ _k ” in the first term on the right side is a state transition matrix that represents the transition of the state vector “x” from step “k” to step “k + 1”. “Wu” in the second term on the right side indicates an input error after discretization, and “wv” in the third term on the right side indicates a process error after discretization.

At this time, the update equation of the error covariance “P” of the state vector “x” can be expressed as the following equation (5).

However, “QU _k ” is a covariance matrix of input error “wu”, and “QV _k ” is a covariance matrix of process error “wv”. What is characteristic in the present embodiment is that in equation (5), as a parameter (error parameter) representing an error, in addition to the covariance matrix “QV _k ” of the process error “wv”, the input error “wu” The covariance matrix “QU _k ” is added.

In this embodiment, “QU _k ” can be adjusted by setting a different input error covariance matrix “Qu” based on the determination result of the user's movement status. Here, the input error covariance matrix “Qu” is a parameter set by the parameter value setting unit 40 based on the determination result of the movement state determination unit 20, and is a parameter based on the error of the measurement values of the acceleration and angular velocity of the user. It is an example. For example, a fixed value can be set for the covariance matrix “QV _k ” of the process error “wv”.

  This will be specifically described. Set dispersion values (parameter values) with different magnitudes depending on the user's movement status. In a walking situation, compared to a situation where the user is stationary, an error is likely to be mixed into the acceleration measured by the acceleration sensor, and the variance (variation) of the error tends to increase. This is because during walking, the user moves while moving his / her arms and legs back and forth and moves up and down, so that the user is easily affected by vibration and impact. Therefore, when the determination result of the movement situation is the walking situation, the error variance value of each axis is set larger than that in the stationary situation.

  In addition, a relatively large variance value is set for the boarding situation as compared to the stationary situation. In the riding situation, even if the user is stationary, the user's body is also affected by the vibration and impact due to the vibration and impact of the moving body. As a result, an error is likely to be mixed in the measurement result of the inertial sensor as compared with the stationary state, and the variance of the error tends to increase.

  Also, for each movement situation, the variance value for acceleration may be set relatively large compared to the variance value for angular velocity. This is because acceleration is more susceptible to errors than angular velocity. That is, the user's posture does not change so much when walking or standing still, and it is unlikely that a large error will be mixed in the angular velocity measured by the gyro sensor.

  In addition, about each component of acceleration and angular velocity, it is good also as setting all the same values as the dispersion value which concerns on an x-axis-z-axis, but it is good also as setting a different value for every coordinate axis. For example, the variance value of the error in the direction of gravity may be set larger than the variance value of the error in the other direction regardless of the movement state. This corresponds to setting the parameter value based on the fact that the variance of the error in the direction of gravity is greater than the variance of the error in the other direction.

  Further, when walking, an error is likely to be superimposed in the direction of gravity, particularly due to the vertical movement of the user's body. Therefore, the variance value of the error in the gravity direction may be set larger when the determination result of the movement situation is the walking situation than in the stationary situation. This is equivalent to setting the parameter value based on the fact that the variance of the error in the direction of gravity is greater when the determination result of the movement situation is the walking situation than in the stationary situation.

  In these cases, for example, the axis closest to the axis in the gravitational direction among the detection axes in the local coordinate system is detected as the sensitivity axis. Then, the error variance value of the detected sensitivity axis is set larger than the error variance value of the other detection axes. In other words, with the inertial calculation device (inertial sensor) attached to the user, the relative relationship between the local coordinate system and the absolute coordinate system is determined, and based on the relative relationship, the error variance value for the detection axis in a specific direction Set a larger value.

  The input error covariance matrix “Qu” is a matrix determined with reference to the local coordinate system. As described above, the input error covariance matrix “Qu” is determined by determining the relative relationship between the local coordinate system and the absolute coordinate system. "Is equivalent to a matrix including parameters related to each axis defining the spatial coordinate system. After determining the sensitivity axis corresponding to the gravity direction, the variance value of the acceleration error with respect to the sensitivity axis is set larger than the variance value of the acceleration error with respect to the other detection axes.

  If the input error covariance matrix “Qu” is set based on the determination result of the movement situation as described above, the prediction of the state vector “x” and the error covariance “P” is performed according to the equations (4) and (5). Perform the operation. Then, at the timing when the observation information is generated and output by the observation information generation unit 30, the state vector “x” and the error covariance “P” according to the conventionally known Kalman filter correction calculation formula (observation update calculation formula). ”Is performed.

2. Example Next, an example of an electronic apparatus to which the inertial navigation calculation device described in the principle is applied will be described. However, it goes without saying that the embodiments to which the present invention can be applied are not limited to the embodiments described below.

  FIG. 3 is a diagram illustrating an example of a schematic configuration of the INS calculation system according to the present embodiment. The INS calculation system includes an inertial navigation calculation device 1000 mounted on a human. The inertial navigation calculation device 1000 is used, for example, by being worn on the user's right waist. When a user performs an exercise such as walking or running, the user exercises with the device attached to the right waist. By storing information such as position historically, users can edit various information such as exercise records by checking the movement route later or connecting the inertial navigation calculation device 1000 to a personal computer or the like. It can be carried out.

  Further, in the present embodiment, not only when the user performs exercises such as walking and running, it is assumed that the inertial navigation calculation device is worn on a daily basis to calculate the position and the like. That is, not only the walking situation but also a situation where the user moves while riding on various moving bodies (automobiles, bicycles, elevators, etc.) is assumed.

  Inertial navigation calculation apparatus 1000 includes IMU 500 as a sensor unit having acceleration sensor 5A and gyro sensor 5B, and measures acceleration and angular velocity in a local coordinate system associated with IMU 500. In addition, the inertial navigation calculation apparatus 1000 includes a reference value generation apparatus, and generates a reference value when correcting detected values of various amounts such as a user position, a velocity vector, and an attitude angle. Then, using the generated reference value, the INS calculation result is corrected as described in the principle.

2-1. Functional Configuration FIG. 4 is a block diagram illustrating an example of a functional configuration of the inertial navigation calculation apparatus 1000. The inertial navigation calculation apparatus 1000 includes a processing unit 100, an operation unit 200, a display unit 300, a sound output unit 400, an IMU 500, an orientation sensor 550, and a storage unit 600 as main functional configurations. Is done.

  The processing unit 100 is a control device and a calculation device that comprehensively control each unit of the inertial navigation calculation device 1000 according to various programs such as a system program stored in the storage unit 600, and is a CPU (Central Processing Unit) or DSP (DSP). (Digital Signal Processor).

  The operation unit 200 is an input device configured by, for example, a touch panel or a button switch, and outputs a pressed key or button signal to the processing unit 100. By operating the operation unit 200, various instructions such as start of exercise and end of exercise are input.

  The display unit 300 is configured by an LCD (Liquid Crystal Display) or the like, and performs various displays based on display signals input from the processing unit 100. The display unit 300 displays information such as a calculation value related to the inertial navigation calculation.

  The sound output unit 400 is a sound output device configured with a speaker or the like, and performs various sound outputs based on the sound output signal output from the processing unit 100. The sound output unit 400 outputs sound guidance, a pace sound related to walking or running, and the like.

  The IMU 500 is a sensor unit known as an inertial measurement unit, and includes, for example, an acceleration sensor 5A and a gyro sensor 5B. The acceleration and angular velocity measured by the IMU 500 are output to the processing unit 100 as needed.

  The direction sensor 550 is a sensor for detecting the moving direction of the user, and includes, for example, a geomagnetic sensor. The moving direction of the user detected by the direction sensor 550 is output to the processing unit 100 as needed.

  The storage unit 600 is configured by a storage device such as a ROM (Read Only Memory), a flash ROM, or a RAM (Random Access Memory), and implements a system program of the inertial navigation calculation device 1000 and various functions such as an INS calculation. Various programs and data are stored. In addition, it has a work area for temporarily storing data being processed and results of various processes.

2-2. The data configuration storage unit 600 stores a main program 610 that is read by the processing unit 100 as a program and executed as a main process (see FIG. 7). The main process will be described later in detail using a flowchart.

  In addition, the storage unit 600 includes, as data, IMU measurement data 620, calculation data 630, calculation error data 640, input error covariance matrix setting data 650, stride model data 660, and walking observation information 670. And boarding observation information 680 and stationary observation information 690 are stored.

  The IMU measurement data 620 is measurement result data of the IMU 500. For example, acceleration (measurement acceleration) measured by the acceleration sensor 5A and angular velocity (measurement angular velocity) measured by the gyro sensor 5B are stored in time series.

  The calculation data 630 is data in which calculation results related to the inertial navigation calculation and calculation values related to the calculation process are stored. This includes in-process data relating to various quantities obtained in the calculation process of the inertial navigation calculation of the INS 10 in FIG. 2 and data such as absolute coordinate positions obtained as calculation results.

  The calculation error data 640 is data in which the INS calculation error calculated by performing the error calculation process is stored. This includes an error of the INS calculation result obtained as a result of the error estimation processing of the error estimation processing unit 50 of FIG.

  The input error covariance matrix setting data 650 is data used to set the input error covariance matrix, and a data configuration example is shown in FIG. In the input error covariance matrix setting data 650, a movement state 650A and a parameter set 650B are defined in association with each other.

  The movement status 650A defines a user's movement status such as a walking status, a boarding status, and a stationary status. In the parameter set 650B, six types of dispersion value sets (parameter value sets) to be set as diagonal components of the input error covariance matrix “Qu” are determined for the corresponding movement state 650A.

  The stride model data 660 is data modeling the stride of the user, and an example of the data configuration is shown in FIG. As a result of experiments, it was discovered that when a human walks, there is a positive correlation between the vertical acceleration and the stride. Therefore, in this embodiment, the stride model data 660 is defined as a stride estimation model equation approximated by a linear function having the vertical acceleration “x” as a variable and the stride “y” as a function value.

  The walking observation information 670 is Kalman filter processing observation information that is set when the movement status of the user is “walking status”. This includes a walking speed vector 670A that is calculated based on the user's stride and one-step required time, and the user's moving direction.

  The on-board observation information 680 is Kalman filter processing observation information that is set when the movement status of the user is “boarding status”. This includes a moving speed restriction speed vector 680A in which speed components other than the moving direction of the moving body are zero.

  The stationary observation information 690 is Kalman filter processing observation information that is set when the movement status of the user is “stationary status”. This includes a stationary time limit speed vector 690A in which the speed component in the absolute coordinate system is zero.

2-3. Processing Flow FIG. 7 is a flowchart showing a flow of main processing executed by the processing unit 100 in accordance with the main program 610 stored in the storage unit 600.

  First, the processing unit 100 performs initial setting (step A1). Specifically, the values of various parameters (calculation parameters for Kalman filter processing) used in error estimation processing are initialized. Further, the detection axis of the inertial sensor closest to the direction of gravity is determined.

  And the process part 100 starts acquisition of data from IMU500, and memorize | stores the acquired data in the memory | storage part 600 as IMU measurement data 620 (step A3). Next, the processing unit 100 performs inertial navigation calculation processing using the IMU measurement data 620, and stores the calculation result in the storage unit 600 as calculation data 630 (step A5).

  Thereafter, the processing unit 100 performs a movement status determination process for determining the movement status of the user based on the measurement result of the IMU 500 and the calculated value related to the inertial navigation calculation process (step A7). Then, the processing unit 100 refers to the input error covariance matrix setting data 650 stored in the storage unit 600, and sets the input error covariance matrix based on the movement status determined in step A7 (step A9). .

  Next, the processing unit 100 performs a Kalman filter prediction calculation according to the equations (4) and (5) based on the input error covariance matrix set in step A9 (step A11). That is, an operation for predicting the state vector “x” and the error covariance “P” is performed.

  Thereafter, the processing unit 100 determines whether or not the movement status of the user is a “walking status” (step A13). When it determines with it not being a "walking condition" (step A13; No), the process part 100 transfers a process to step A17.

  On the other hand, when it determines with it being a "walking condition" (step A13; Yes), the process part 100 calculates the speed vector 670A at the time of a walk, and sets it as the observation information 670 at the time of a walk (step A15).

  Specifically, a peak determination is made for vertical acceleration, and the time from the previous acceleration peak to the current acceleration peak is calculated as the time required for one step. The user's stride is determined with reference to the stride model data 660 stored in the storage unit 600. Then, based on the time required for one step and the step length and the moving direction of the user detected by the azimuth sensor 550, the user's speed vector during walking (speed vector during walking) is calculated.

  Next, the processing unit 100 determines whether or not the movement status of the user is “boarding status” (step A17), and if it is determined that the movement status is not “boarding status” (step A17; No), the process proceeds to step A21. And migrate the process. In addition, when it is determined that the state is “boarding state” (step A <b> 17; Yes), the processing unit 100 sets the travel time restricted speed vector 680 </ b> A as the boarding time observation information 680 (step A <b> 19).

  Next, the processing unit 100 determines whether or not the movement status of the user is “stationary status” (step A21). If the processing unit 100 determines that the movement status is not “stationary status” (step A21; No), the processing unit 100 proceeds to step A25. And migrate the process. When it is determined that the state is “still state” (step A21; Yes), the processing unit 100 sets the stationary restriction speed vector 690A as the stationary observation information 690 (step A23).

  Next, the processing unit 100 executes a Kalman filter correction calculation (step A25). Specifically, the correction calculation based on the theory of the Kalman filter is performed using the observation information set in any of steps A15, A19, and A23. Then, the calculation result is stored in the calculation error data 640 as a calculation error.

  Next, the processing unit 100 outputs the INS calculation result obtained in Step A5 (Step A27). And the process part 100 determines whether a process is complete | finished (step A29), and when it determines with continuing a process (step A29; No), a correction process is performed (step A31). That is, each calculation result included in the calculation data 630 is corrected using an error (correction amount) of each calculation result stored in the calculation error data 640.

  On the other hand, when it determines with complete | finishing a process in step A29 (step A29; Yes), the process part 100 complete | finishes a main process.

3. According to the present embodiment, the movement status of the user is determined, and the parameter value related to the predetermined error estimation process for estimating the error included in the calculated value related to the predetermined inertial navigation calculation is determined based on the determination result of the moving status. Set based on. Specifically, the value of the input error covariance matrix “Qu”, which is a parameter used in the Kalman filter process, is set in accordance with the movement status of the user. Thereby, based on a user's movement condition, the value of the parameter concerning error estimation processing can be optimized. By executing the error estimation process using the optimized parameter values, it is possible to correctly estimate the error included in the calculated value. If the error estimated in this way is used, the calculation value related to the inertial navigation calculation can be corrected appropriately.

  When the input error covariance matrix “Qu” is a fixed value, the following problem occurs. When the error covariance “P” is calculated to be excessively larger than the true value, the filter acts so as to trust the observed value “z” rather than the value of the state vector “x” one step before, and the obtained state A phenomenon occurs in which the estimated value of the vector “x” is dragged to the observed value. On the other hand, when the error covariance “P” is calculated to be excessively smaller than the true value, the filter acts to trust the value of the state vector “x” one step before the observed value “z”, There arises a problem that the state estimation value becomes unstable.

  The magnitude of the error (noise) included in the measurement value of the inertial sensor (inertia calculation device) attached to the user's body changes depending on the movement state of the user. During walking, it is subject to vibrations and shocks constantly, so errors are particularly likely to be mixed into the measured values in the direction of gravity. On the other hand, when the user is stationary or riding on a moving body, it is difficult for errors to be mixed in the measured values in the direction of gravity compared to walking. Based on this tendency, the error included in the input of the Kalman filter is compensated by making the input error covariance matrix “Qu” variable according to the movement state of the user, and the error covariance “P” of the state vector “x”. Can be brought closer to the true value. As a result, the above problem is solved, and it is possible to correctly estimate the error of the inertial navigation calculation.

4). Modifications Embodiments to which the present invention can be applied are not limited to the above-described embodiments, and can be changed as appropriate without departing from the spirit of the present invention. Hereinafter, modified examples will be described.

4-1. Observation Information It is also possible to perform error estimation in the same manner as in the above embodiment using information such as position and velocity vector calculated using a satellite positioning system as observation information.

  For example, the user's position and velocity vector are calculated using GPS, which is a kind of satellite positioning system. Then, at the output timing of the GPS calculation result, the correction calculation based on the Kalman filter theory may be performed using the position and velocity vectors as observation information. In addition, since the calculation method of a position and a velocity vector is conventionally well-known, detailed description is abbreviate | omitted here.

  In addition, information such as position information may be acquired from an RF (Radio Frequency) tag and used as observation information. In this case, the inertial navigation calculation device is provided with a tag reader for reading position information embedded in the RF tag. Then, position information can be acquired by placing an inertial navigation calculation device on an RF tag placed in advance at a predetermined position, and this position information can be used as observation information to perform correction calculations based on the Kalman filter theory. Good.

4-2. Parameter Value Setting In the above-described embodiment, the input error covariance is fixedly set as the parameter value corresponding to the movement state of the user. However, there are situations where it is difficult to determine the user's movement status. For example, when the user is operating while riding on a moving body, it may be difficult to determine whether the user's movement status is a walking situation or a boarding situation.

  Therefore, in such a case, the covariance of two or more types of input errors may be mixed and set. For example, when it is determined that the possibility of the situation where the user is walking and the situation where the user is on a moving body is a fifth, the covariance of the input error predetermined for the walking situation is Alternatively, the covariance of the input error determined in advance with respect to the boarding situation may be averaged and the value may be set.

4-3. Processing Subject In the above embodiment, the IMU is mounted on the electronic device, and the processing unit of the electronic device performs inertial navigation calculation processing based on the measurement result of the IMU. However, the INS may be mounted on the electronic device, and the processing unit of the INS may perform inertial navigation calculation processing. In this case, for example, the processing unit of the electronic device performs a process of estimating an error (inertial navigation calculation error) included in the inertial navigation calculation result output from the INS. Then, the inertial navigation calculation result input from INS is corrected using the estimated inertial navigation calculation error.

  In addition, a sensor unit to which a satellite positioning system such as GPS (Global Positioning System) is applied may be mounted on the electronic device, and the inertial navigation calculation result may be corrected using the measurement result of the sensor unit.

4-4. In the above-described embodiment, the case where the present invention is applied to the inertial navigation calculation device mounted on the waist has been described as an example. However, the electronic device to which the present invention is applicable is not limited thereto. For example, the present invention can be applied to other electronic devices such as a portable navigation device (portable navigation), a portable phone, a personal computer, and a PDA.

  DESCRIPTION OF SYMBOLS 1 Overall system, 5A Acceleration sensor, 5B Gyro sensor, 10 INS, 11 Attitude information calculation part, 13 Absolute coordinate acceleration vector calculation part, 15 Absolute coordinate speed vector calculation part, 17 Absolute coordinate position calculation part, 18 Gravity calculation part, 19 Correction unit, 20 Movement status determination unit, 30 Observation information generation unit, 40 Parameter value setting unit, 50 Error estimation processing unit, 100 Processing unit, 200 Operation unit, 300 Display unit, 400 Sound output unit, 500 INS, 550 Direction sensor , 600 storage unit, 1000 inertial navigation calculation device

Claims (7)

Determining the user ’s travel status,
A parameter value related to a predetermined error estimation process for estimating an error included in any one of a calculation result and a calculation process value related to a predetermined inertial navigation calculation (hereinafter referred to as “calculated value”) Setting based on the judgment result;
Performing the error estimation process using the value of the parameter;
Performing the inertial navigation calculation by correcting the calculation value using the error estimated by the error estimation process;
Inertial navigation calculation method. The inertial navigation calculation is a navigation calculation using a measured value of acceleration of the user,
The error estimation process is a Kalman filter process in which the parameter is a parameter based on an error of the measurement value.
The inertial navigation calculation method according to claim 1. The parameter includes a parameter related to each axis defining a predetermined spatial coordinate system,
The setting includes setting a value of the parameter based on a variance of errors in the gravitational direction being greater than a variance of errors in other directions;
The inertial navigation calculation method according to claim 2. Determining the movement situation includes determining at least whether it is a walking situation or a stationary situation,
The setting includes setting the value of the parameter based on the fact that the variance of the error in the gravitational direction is larger when the determination result of the movement situation is the walking situation than in the stationary situation. ,
The inertial navigation calculation method according to claim 3. Determining the movement situation includes determining whether it is at least a walking situation;
When the determination result of the movement situation is a walking situation, detecting the user's stride and the time required for one step based on a measurement value of acceleration in the vertical direction of the user;
Calculating the speed vector of the user during walking based on the stride and the time required for one step and the moving direction of the user;
Including
The error estimation process is a Kalman filter process in which the parameter is a parameter based on the error of the measurement value and the velocity vector is an observed value when the determination result of the movement situation is a walking situation.
The inertial navigation calculation method according to any one of claims 2 to 4. Determining the movement status includes determining whether at least the user is on a moving body,
The error estimation process is a Kalman filter process in which the parameter is a parameter based on the error of the measurement value and a predetermined restricted velocity vector is an observed value when the determination result of the movement condition is a situation on a moving object Is,
The inertial navigation calculation method according to any one of claims 2 to 5. A determination unit for determining the movement status of the user;
A setting unit configured to set a parameter value related to a predetermined error estimation process for estimating an error included in a calculated value related to a predetermined inertial navigation calculation based on the determination result of the movement state;
An error estimation processing unit that executes the error estimation process using the value of the parameter;
An inertial navigation calculation unit that corrects the calculation value using the error estimated by the error estimation process and performs the inertial navigation calculation;
Inertial navigation calculation device equipped with.

Images