JP2017106891A - Inertia device, program, and positioning method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide an inertia device that increases the accuracy of positioning in a building.SOLUTION: An inertia device 1 includes: a running direction estimation unit 200, that estimates a state of running, including a running direction and a moving rate, using a sensor output; a position estimation unit 800, that estimates the position of an own device by applying the running direction and the moving rate, estimated by the running direction estimation unit, to an inertial navigation; an absolute position information acquisition unit, that acquires absolute position information, provided from the outside; a position correction unit 500, that generates a corrected position, obtained by correcting an estimated position by the absolute position information, in a case where the absolute position information has been acquired; and a running direction correcting unit 106, that corrects the running direction based on a difference in the direction with respect to a predetermined position between from the estimated position and from the corrected position.SELECTED DRAWING: Figure 35

Description

  The present invention relates to an inertial apparatus, a program, and a positioning method.

  Inertial navigation technology (PDR; Pedestrian) that estimates the position of a pedestrian using the detection values of an acceleration sensor, gyro sensor, and geomagnetic sensor in an environment where positioning using GPS (Global Positioning System) cannot be performed, such as indoors or underground Dead Reckoning) is known. In the inertial navigation technique, the current position is estimated by estimating a pedestrian's traveling direction and walking speed vector by calculating a detected value, and time-integrating the walking speed vector with respect to the traveling direction. A sensor in which an acceleration sensor, a gyro sensor, and a geomagnetic sensor are integrated is called a motion sensor.

  However, the inertial navigation technology for pedestrians requires the user to perform calibration operations such as stride and walking speed before using the device in order to improve position estimation accuracy. Further, a technique for learning parameters for estimating a stride and a walking speed when absolute position information such as GPS is acquired is known, but it does not function unless it is outdoors.

  In the azimuth estimation in the inertial navigation technique, the pedestrian travel direction is determined based on the absolute azimuth angle detected by a geomagnetic sensor, a gyro sensor, or the like. That is, if the absolute azimuth angle is not known by the geomagnetic sensor, the current position cannot be estimated correctly even if the amount of change in azimuth is known by the gyro sensor.

  However, it is known that the detection error of absolute azimuth is remarkably large in indoor environments such as buildings and basements. This is because the geomagnetic sensor is disturbed by an environmental magnetic field from a structure such as a reinforcing bar, and inertial navigation based only on the geomagnetic sensor is not practical in an indoor environment.

  As another problem, a zero point shift (generally referred to as a gyro drift) due to a temperature change occurs in the gyro sensor. Since the zero point shift causes a decrease in azimuth estimation accuracy due to superposition of accumulated errors, it is difficult to continuously detect the azimuth accurately without using a geomagnetic sensor.

  There is a known method of correcting the azimuth angle estimated by the gyro sensor with the absolute azimuth angle detected by the geomagnetic sensor when the geomagnetic sensor and the gyro sensor are hybridized and the azimuth angle detected by the geomagnetic sensor is determined to be reliable. . However, in an indoor environment where the orientation detected by the geomagnetic sensor cannot be trusted, there are many situations where the correction itself is not performed, and there is a problem common to the case where only the gyro sensor is used.

  As a technique for improving the accuracy of the absolute direction, a technique using map matching is known (for example, see Patent Document 1). Patent Document 1 discloses a technique for detecting a straight walk by inertial navigation and correcting the user's direction from the direction between two points matched with map information.

  However, the orientation correction method described in Patent Document 1 is not only a problem that map information is required, but also correct orientation correction when matching to an incorrect location on the map due to a decrease in location accuracy due to inertial navigation. There is a problem that it cannot be done.

  In view of the above problems, an object of the present invention is to provide an inertial device that improves indoor positional accuracy.

  In view of the above problems, the present invention applies a traveling direction estimation unit that estimates a traveling state including a traveling direction and a traveling speed using sensor output, and a traveling direction and a traveling speed estimated by the traveling direction estimation unit to inertial navigation. If the absolute position information is acquired, the position estimation unit that estimates the estimated position of the device itself, the absolute position information acquisition unit that acquires the absolute position information provided from the outside, and the absolute position information are corrected. The inertial device 1 includes a position correction unit that generates a correction position and a traveling direction correction unit that corrects the traveling direction based on a difference in direction from the estimated position and the correction position with respect to a predetermined position.

  An inertial device that improves indoor positional accuracy can be provided.

It is an example of the figure explaining the error of the distance between two points and azimuth | direction error which arise by inertial navigation. It is an example of the figure explaining the coordinate system of the inertial apparatus. It is a figure showing the hardware structural example of the inertial apparatus in one Embodiment of this invention. It is a figure showing a part of functional block of the inertial device in one Embodiment of this invention. It is a figure explaining the item contained in the state estimation model of the system in the general formula of an extended Kalman filter. It is a figure explaining the partial differential matrix (Jacvian) in the time update in the general formula of an extended Kalman filter. It is a figure explaining the error covariance matrix _{Pk | k-1} in the general formula of an extended Kalman filter. It is a figure explaining the observation error in the general formula of an extended Kalman filter. It is a figure explaining the observation error in the general formula of an extended Kalman filter. It is an example of the flowchart figure of the procedure which calculates 2nd attitude | position information by the attitude | position calculating part 104TRIAD algorithm. It is an example of the figure explaining coordinate transformation. It is an example of the figure explaining rotation matrix DCM2. It is an example of the figure explaining the function of the advancing direction estimation part. It is an example of the figure which shows the waveform showing the change of the vertical component moving acceleration (Z) and the horizontal component (X, Y) of a moving acceleration. It is an example of the figure explaining the motion characteristic of the perpendicular direction in walking motion. It is an example of the figure explaining the movement characteristic of the horizontal direction in walking operation. It is an example of the figure which shows the waveform showing the change with respect to time of the horizontal component of a movement acceleration, and a vertical component. It is a figure which shows an example of a horizontal component moving speed feature vector. It is an example of the flowchart figure which shows the procedure which determines whether various information is the data acquired derived from walking motion. It is an example of the figure explaining the fluctuation width of the moving speed on either side. It is an example of the figure explaining the advancing direction estimation process. It is an example of the figure explaining the function which a position estimation part has. It is an example of the detailed functional block diagram inside a progress speed estimation part. It is an example of the figure showing an example of a parameter database. It is an example of the figure showing the example of a progress velocity waveform. It is an example of the figure showing the example of the synthetic | combination progress velocity waveform. It is an example of the functional block diagram of the absolute position information input part of an inertial apparatus, an absolute position estimation part, and a map matching part. It is an example of the figure showing the relationship between a radio wave intensity and a positioning time interval. It is an example of the figure explaining the calculation of the present position in the procedure of time development. It is an example of the figure explaining the calculation of the present position in the procedure of time development. It is an example of the figure explaining the calculation of the present position in the procedure of time development. The figure explaining the calculation of the present position in the procedure of observation update. It is an example of the figure which shows the speed information after correction | amendment typically. It is a figure which shows an example of the change of the received signal strength according to the distance of the transmitter of an absolute position information, and an inertial apparatus. It is an example of the flowchart figure which shows the estimation procedure of a walking speed estimation parameter and a yaw angle correction value. It is an example of the figure which shows the mode of the positioning result obtained when moving the five receiving areas, and the mode of correction | amendment. It is an example of the figure which compares the movement locus | trajectory of the pedestrian obtained by the prior art and the technique of this embodiment. The figure explaining the general formula of an extended Kalman filter (prior art). The figure explaining the variable used in the procedure of time update (prior art). The figure explaining the variable used in the procedure of observation update (prior art).

  Hereinafter, embodiments for carrying out the present invention will be described with reference to the drawings.

  First, the error of the distance between two points and the azimuth | direction error which arise by inertial navigation are demonstrated using FIG. FIG. 1 is an example for explaining a distance error between two points and an azimuth error caused by inertial navigation.

  The reception area AREA1 and the reception area AREA2 indicate areas where the absolute position information can be received wirelessly such as radio waves and sound waves when the inertial device enters. P1 and P2 are absolute positions included in the absolute position information. Absolute position information is a position (latitude, longitude, altitude, etc.) that has been confirmed to be accurate according to some standard (coordinate system), and is periodically transmitted by a transmitter installed on the ceiling or passage. ing.

  P′1 indicates a position corrected by a parametric method (Kalman filter or the like) in the absolute position information reception area AREA1, and P′2 indicates a position corrected by a parametric method in the absolute position information reception area AREA2. Circles E1 and E2 are error estimation values (error information) estimated by a parametric method.

  Consider a case where a pedestrian carrying the inertial device 1 moves from the reception area AREA1 to the reception area AREA2. In the reception area AREA1, the position of the pedestrian is corrected to P'1, and this is the starting point. When the pedestrian arrives at the reception area AREA2, the trajectory 1012 of the movement only by inertial navigation and the estimated position 1001 are greatly deviated from P2. It is the estimation error of distance and direction that is caused by the deviation. That is, when the walking speed parameter for estimating the walking speed is not correct, or when the orientation estimated by the inertial device 1 includes an error, the position 1001 estimated only by the inertial navigation is corrected by a parametric method. It is deviated from the position P′2. FIG. 1 shows an azimuth estimation error (yaw angle correction value) Δψ and a distance estimation error ΔL.

  The reason that these estimation errors occur is that the walking speed estimation parameter for estimating the walking speed of the pedestrian is not appropriate for the pedestrian, and the error is superimposed on the position 1001 obtained by integration, This is because the azimuth (yaw angle) estimated by the inertial device 1 is not corrected. Therefore, there is a demand for an inertial device 1 that can correct these accurately and perform more accurate position estimation.

<Outline of positioning method by inertial device of this embodiment>
Then, the outline of the positioning method by the inertial apparatus 1 of this embodiment is demonstrated. The positioning method of the present embodiment is obtained when the absolute position information is received as a positioning result obtained by inertial navigation (including at least position coordinates. If parametric techniques are used for inertial navigation, error information is also included). Correction is performed using a parametric method, and errors (E1 and E2 in FIG. 1) with respect to the position coordinates are estimated.

Position coordinates obtained by inertial navigation are reset with the corrected position coordinates and error. Then, when a “walking condition” described later is satisfied, the following processing is executed.
1. The distance A between the two points of the latest corrected position coordinate (P′2) and the previous corrected position coordinate (P′1) and the traveling direction (first direction connected by a straight line of the distance A) are calculated.
2. The position coordinate (P′1) after the previous correction and the distance B of the position 1001 obtained only by inertial navigation and the traveling direction between the two points (the second direction connected by the straight line of the distance B) are calculated.
3. The result of dividing the distance A by the distance B is used as a walking speed correction parameter for the subsequent position estimation by inertial navigation.
The posture is corrected using the difference between the first azimuth of 4.1 and the second azimuth of 2 as a yaw angle correction value (direction correction parameter) Δψ.

  According to such a positioning method, the walking speed correction parameter and the direction correction parameter are calculated by a parametric method, and positioning by the subsequent inertial navigation is performed at the corrected walking speed in the corrected traveling direction. The position can be estimated with high accuracy.

Note that the parametric method is a method for estimating a parameter that defines the characteristics of a population by assuming a hypothesis. For example, a true value or an error is estimated assuming normality or equal variance of the population. Say the technique. In contrast to this, there is a non-parametric method, which is a method for estimating true values and errors without making any assumptions about the distribution type (parameter) of the population. In this embodiment, a Kalman filter is used as an example of a parametric method.

<Outline of inertial device>
FIG. 2 is an example of a diagram for explaining the coordinate system of the inertial device 1. 2A shows an absolute coordinate system described later, and FIG. 2B shows a device coordinate system fixed to the inertial apparatus 1. In this specification, as an absolute coordinate system, a plane on which a user walks is represented by an X axis and a Y axis, and a vertical direction with respect to the X axis and the Y axis is represented by a Z axis. The X axis and the Y axis are vertical. The device coordinate system is represented by the X ′ axis, the Y ′ axis, and the Z ′ axis.

  The inertial apparatus 1 has a multiple called an information processing apparatus. For example, a user such as a terminal called a smart device, a mobile phone, a smartphone, a tablet terminal, a wearable PC, a PDA (Personal Digital Assistant), or a notebook PC Is a small device that can be carried around. The inertial device 1 includes an inertial sensor (a triaxial acceleration sensor, a triaxial angular velocity sensor) and a triaxial geomagnetic sensor that are mounted on a commercially available smartphone or the like. The inertial device 1 can detect changes and directions of acceleration and angular velocity using each sensor. The geomagnetic sensor detects geomagnetism and detects the azimuth as an absolute value. The acceleration sensor detects a change in acceleration. The angular velocity sensor detects a change in angular velocity. An inertial sensor is a general term for sensors that detect a change in inertia and output a signal corresponding to the magnitude of the change.

  The inertial apparatus 1 may be a device other than the devices exemplified above (for example, a music player, an activity meter, a wristwatch, etc.). The inertial apparatus 1 may be provided by being incorporated in another device (for example, a walking robot, a collar attached to an animal, or the like). This makes it possible to estimate the traveling directions of various people or objects that move in the vertical direction at a constant cycle.

<Hardware configuration>
Next, the hardware configuration of the inertial device 1 according to an embodiment of the present invention will be described with reference to FIG. FIG. 3 is a hardware configuration example when the inertial apparatus 1 is a smart device such as a smartphone.

  Inertial device 1 includes CPU 11, RAM 12, ROM 13, acceleration sensor 14, angular velocity sensor 15, geomagnetic sensor 16, microphone 17, speaker 18, communication module 19, Bluetooth (registered trademark) communication module 20, GPS reception module 21, display 22, A touch panel 23, a battery 24, an atmospheric pressure sensor 25, and a bus 26 are included.

The CPU 11 executes a program that controls the operation of the inertial device 1. The RAM 12 constitutes a work area of the CPU 11 and the like. The ROM 13 stores a program executed by the CPU 11 and data necessary for executing the program. The acceleration sensor 14 detects acceleration (sensor output) in the X ′, Y ′, and Z ′ axis directions in the device coordinate system of the inertial apparatus 1. The angular velocity sensor 15 (or gyro sensor) detects the angular velocity (sensor output) of the inertial apparatus 1 in the X ′, Y ′, and Z ′ axis directions in the device coordinate system. The geomagnetic sensor 16 outputs a three-dimensional vector (sensor output) representing magnetic north and detects the direction of the inertial device 1. The atmospheric pressure sensor 25 measures the atmospheric pressure and detects the altitude of the inertial device 1.

  The microphone 17 converts a voice such as a user's voice into an electrical signal. The speaker 18 outputs an electrical signal as sound. The communication module 19 is a device for communicating with other devices connected to the 3G network and / or the wireless LAN. The Bluetooth (registered trademark) communication module 20 is a device for performing communication using Bluetooth (registered trademark). The GPS receiving module 21 is a device for receiving a positioning signal transmitted by a GPS satellite or IMES (Indoor Messaging Service).

  The display 22 is a device for presenting a screen to the user. The touch panel 23 is a device that receives input from the user. The battery 24 is a device that supplies electric power for driving the inertial device 1. The bus 26 connects the devices except for the battery 24 to each other.

  Note that the microphone 17, the speaker 18, the communication module 19, the Bluetooth (registered trademark) communication module 20, the GPS reception module 21, the display 22, and the touch panel 23 are optional components. For example, when the inertial apparatus 1 is a device such as an activity meter having no display screen, these apparatuses may not be provided.

  Further, the inertial device 1 may include a device (for example, a ZigBee (registered trademark) communication module) that performs wireless communication according to another standard instead of the Bluetooth (registered trademark) communication module 20.

<Overall configuration of inertial device>
FIG. 4 is an example of a functional block diagram of the inertial device 1. The inertial apparatus 1 includes a posture information conversion processing unit 100, a traveling direction estimation unit 200, a correction parameter estimation processing unit 700, a position estimation unit 800, a posture information estimation unit 110, a device coordinate system triaxial acceleration acquisition unit 120, and a positioning error. A coordinate estimation unit 330 is included. These units included in the inertial device 1 are functions or means realized by the CPU 11 shown in FIG. 3 executing a program expanded from the RAM 12 to the ROM 13 and controlling the units shown in FIG.

  The correction parameter estimation processing unit 700 calculates correction parameters (a yaw angle correction value 704 and a walking speed estimation parameter 705). Details will be described later.

  The posture information estimation unit 110 acquires acceleration, angular velocity, and detected values of geomagnetism, and creates a quaternion (having information equivalent to a direction cosine matrix or a rotation matrix DCM1 to be described later). The quaternion is notified to the posture information conversion processing unit 100.

  The posture information conversion processing unit 100 converts the acceleration of the device coordinate system into an absolute coordinate system using the direction cosine matrix (or rotation matrix DCM1). Also, the posture information conversion processing unit 100 acquires the yaw angle correction value 704 and corrects the traveling direction. The travel direction estimation unit 200 is notified of the absolute coordinate system acceleration vector 107 with the travel direction corrected in this way.

  The traveling direction estimation unit 200 creates the traveling direction 213 of the pedestrian using the absolute coordinate system acceleration vector 107 converted into the absolute coordinate system. Details will be described later.

  The position estimation unit 800 calculates a traveling direction velocity vector from the traveling direction 213 estimated by the traveling direction estimation unit 200, and calculates an absolute position and error information using a parametric method such as a Kalman filter. At this absolute position, the speed and position can be obtained only by inertial navigation. Further, when the absolute position information is acquired, the position estimation unit 800 corrects the position using a parametric method such as a Kalman filter and calculates error information. At this time, the absolute position correction flag becomes TRUE. The absolute position, error information, and absolute position correction flag (706) are notified to the correction parameter estimation processing unit 700.

  Further, the position estimation unit 800 acquires the walking speed estimation parameter 705 from the correction parameter estimation processing unit 700 and corrects the traveling speed vector.

  The device coordinate system triaxial acceleration acquisition unit 120 acquires the device coordinate system acceleration from the triaxial acceleration sensor. The three-axis acceleration in the device coordinate system is notified to the coordinate system conversion processing unit 106.

  The positioning error / coordinate estimation unit 330 acquires absolute position information (which may be referred to as absolute position correction information because it is absolute position information for correcting the position) from the transmitter 340 and the like, extracts the absolute position, and determines the positioning error. Is estimated. The positioning error and the absolute position are notified to the position estimation unit 800.

<< Attitude information estimation unit 110 >>
The function of the posture information estimation unit 110 will be described. The posture information estimation unit 110 includes an acceleration acquisition unit 101, an angular velocity acquisition unit 102, a geomagnetism acquisition unit 103, and a posture calculation unit 104. Attitude information estimation section 110 outputs rotation matrix DCM1 (or direction cosine matrix, quaternion) indicating the attitude of the inertial device.

  The absolute coordinate system is a unified coordinate system that is used to uniformly handle coordinate values observed by a plurality of types of sensors. For example, the WGS84 longitude and latitude coordinate system used in GPS, UTM There are Cartesian coordinate systems such as (Universal Transverse Mercator projection). The absolute coordinate system is also called the world coordinate system. The device coordinate system is also called a body coordinate system. One point on the inertial device 1 is defined as an origin, and three axes orthogonal to each other at the origin are defined as an X ′ axis, a Y ′ axis, and a Z ′ axis, respectively. Represents the coordinate system.

  The acceleration acquisition unit 101 acquires a change in triaxial acceleration detected by the acceleration sensor 14. The angular velocity acquisition unit 102 acquires a change in the triaxial angular velocity detected by the angular velocity sensor 15. The angular velocity acquired here is fixed to the device coordinate system, similarly to the acceleration. The geomagnetism acquisition unit 103 acquires a three-dimensional geomagnetic vector representing magnetic north detected by the geomagnetic sensor 16 and acquires information indicating the direction of the inertial device 1. The orientation acquired here is fixed to the device coordinate system, like acceleration.

  The posture calculation unit 104 calculates the current posture of the inertial device 1 using the sensor information acquired by the acceleration acquisition unit 101, the angular velocity acquisition unit 102, and the geomagnetism acquisition unit 103, and the posture information (rotation matrix). From this, an inverse rotation matrix is obtained by inverse matrix calculation. This inverse rotation matrix is a rotation matrix DCM1 described later.

  The posture calculation unit 104 obtains a matrix representing the posture of the inertial device 1 using a generally known extended Kalman filter (Non-Patent Document 1, Non-Patent Document 2, FIG. 38-40), and its inverse matrix. Is output. Hereinafter, the process will be described in detail.

((General formula of extended Kalman filter))
FIG. 38 shows a general expression of a generally known extended Kalman filter. In the Kalman filter, two steps of time evolution and observation update are executed to advance one time step. In the time development procedure, the estimated state of the current time is calculated from the estimated state of the previous time. Also, in the observation update procedure, the observation at the current time is used, the estimated value is corrected, and a more accurate state is estimated. By repeating these procedures sequentially, the optimum state variable is estimated.

  FIG. 39 is a diagram for explaining each variable used in the time evolution procedure of the extended Kalman filter. Each variable is explained in correspondence with the expressions (1) to (3) included in the item of time development in FIG. Here, k represents a discrete step time, and k−1 represents a time one step before the current time.

  FIG. 40 is a diagram for explaining each variable used in the observation update procedure of the extended Kalman filter. Each variable is explained corresponding to the equations (1) to (6) included in the observation update item of FIG.

((Apply extended Kalman filter))
The posture calculation unit 104 uses the “temporal development procedure” in the extended Kalman filter to update posture information based on the output of the angular velocity sensor 15 (roll angle, pitch angle, yaw angle). Further, the “observation update procedure” in the extended Kalman filter is used for updating the posture information by the output of the acceleration sensor 14 (roll angle, pitch angle) (hereinafter referred to as “first observation update procedure”). Further, the “observation update procedure” is also used to update the attitude information by the output of the geomagnetic sensor 16 (yaw angle) (hereinafter referred to as “second observation update procedure”).

  In this way, the posture calculation unit 104 constructs a seven-state extended Kalman filter. The posture calculation unit 104 loops one time development procedure and two observation update procedures in parallel, and estimates the posture and the gyro zero point bias value. Here, the posture is represented by the following quaternion (hereinafter referred to as a quaternion vector).

クォータニオンベクトルとは、４変数からなるベクトルであり、物体の姿勢を表す。ロール・ピッチ・ヨーの姿勢表現ではシンバルロックという特異点問題がある反面、クォータニオンでは、特異点が無く全ての姿勢を表すことができる。また、ジャイロゼロ点バイアス値は、三軸に対応する三変数（ｂｘ_k,ｂｙ_k,ｂｚ_k）によって表される（ｂは定数）。

A quaternion vector is a vector composed of four variables and represents the posture of an object. While roll, pitch, and yaw posture expressions have a singularity problem called cymbal lock, quaternions can represent all postures without singularities. The gyro zero point bias value is represented by three variables (bx _k , by _k , bz _k ) corresponding to the three axes (b is a constant).

  In the following, each of the three procedures (1) to (3) will be described.

((Procedure for time development))
First, the procedure of time evolution in the extended Kalman filter will be described with reference to FIGS. The posture calculation unit 104 executes the procedure, and performs time integration in accordance with the time evolution procedure of the extended Kalman filter with the gyro output value as an input in a state estimation model described later. Then, an updated quaternion vector q and an error covariance matrix P are obtained (roll pitch pitch yaw angle).

  FIG. 5 is a diagram for explaining items included in (1) the system state estimation model in the general expression of the extended Kalman filter in the present embodiment. Here, the state estimated value at the current time is defined as shown in Equation (1) -1 in FIG. 5 by the quaternion vector and the gyro zero point bias value.

また、入力量ｕ_kを、角速度センサの出力値（ω_0xk,ω_0yk,ω_0zk）（rad/sec）を用いて、図５の式（１）−４のように定義する（ｂは定数）。

Also, the input quantity u _k, the output value of the angular velocity sensor _{(ω 0xk, ω 0yk, ω} 0zk) (rad / sec) using a definition to (b as in equation (1) -4 in Fig. 5 is a constant ).

すなわち、（ω_xk,ω_yk,ω_zk）は、ゼロ点の値を減算し、オフセットがない角速度を表す（rad/sec）。そして、システムの状態推定モデルを、図５の式（１）−５のように表す。ここで、Ｃ₁、Ｃ₂、Ｃ₃は、任意の定数である。

That is, (ω _xk , ω _yk , ω _zk ) represents an angular velocity without any offset by subtracting the zero point value (rad / sec). Then, the system state estimation model is expressed as in Expression (1) -5 in FIG. Here, C ₁ , C ₂ , and C ₃ are arbitrary constants.

図６は、本発明において、拡張カルマンフィルタの一般式における、（２）時間更新における偏微分行列（ヤコビアン）を説明する図である。図５を用いて説明したように、システムの状態推定モデルは、図５の式（１）−５によって表される。式（１）−５の右辺はｆであるため、右辺の偏微分が、時間発展における偏微分行列となる。

FIG. 6 is a diagram for explaining a partial differential matrix (Jacvian) in (2) time update in the general formula of the extended Kalman filter in the present invention. As described with reference to FIG. 5, the system state estimation model is represented by Expression (1) -5 in FIG. 5. Since the right side of Equation (1) -5 is f, the partial differentiation of the right side is a partial differentiation matrix in time evolution.

FIG. 7 is a diagram for explaining (3) error covariance matrix P _{k | k−1} in the general expression of the extended Kalman filter in the present invention. The process noise Q _k is a constant obtained by system identification and is calculated in advance.

上記のプロセスノイズＱ_kと、１ステップ前の時刻の誤差共分散行列と、時間発展における偏微分行列（ヤコビアン）Ｆ_k及びその転置行列Ｆ_k ^Tにより、現在時刻の誤差共分散行列Ｐ_k|k-1が求まる（図７の式（３）−５）。なお、現在時刻の誤差共分散行列Ｐ_k|k-1及びＰ_k-1|k-1は、７×７の行列となり、行列内の要素は実数となる。

The error covariance matrix P _{k | at the} current time is calculated from the process noise Q _k , the error covariance matrix at the time one step before, the partial differential matrix (Jacobiane) F _k in time evolution and its transposed matrix F _k ^T. _k-1 is obtained (formula (3) -5 in FIG. 7). Note that the error covariance matrices P _{k | k-1} and P _{k-1 | k-1 at} the current time are 7 × 7 matrices, and the elements in the matrix are real numbers.

  The posture calculation unit 104 executes the time evolution procedure in the extended Kalman filter using the above model and variable definitions, obtains the posture of the inertial device 1 in the absolute coordinate system, and the inverse matrix ( Inverse rotation matrix).

((First observation update procedure))
Next, the first observation update procedure in the extended Kalman filter will be described with reference to FIG. The posture calculation unit 104 executes the procedure, compares the horizontal angle information obtained from the acceleration acquisition unit 101 and the horizontal angle information of the current quaternion vector, and performs a process of correcting the error (roll).・ Pitch angle only).

FIG. 8 is a general expression of the extended Kalman filter.
(1) Observation residual

に含まれる項目を説明する図である。まず、１ステップ前の時間の観測値（ベクトル）ｈは、図８の式（１）−３のように表される。

It is a figure explaining the item contained in. First, the observed value (vector) h at the time one step before is expressed as Expression (1) -3 in FIG.

上記の式に含まれる要素は、三次元の回転行列（４×４）に由来するものであり、予め決められているものとする。また、観測値（ベクトル）ｚ_kは、図８の式（１）−２のように表される。

The elements included in the above expression are derived from a three-dimensional rotation matrix (4 × 4) and are determined in advance. Further, the observation value (vector) z _k is expressed as in Expression (1) -2 in FIG.

ここで、（ａ_x，ａ_y，ａ_z）は、加速度取得部１０１が取得した、加速度センサ１４の出力値である。上記のｈ及びｚ_kにより、観測残差

Here, (a _x , a _y , a _z ) are output values of the acceleration sensor 14 acquired by the acceleration acquisition unit 101. Observation residual by h and z _k above

が求まる。

Is obtained.

Further, in the general expression of the extended Kalman filter, (2) the partial differential matrix (Jacvian) H _k in the observation update is obtained by obtaining the partial differential of the observed value h represented by the expression (1) -3 in FIG.

Further, in the general expression of the extended Kalman filter, (3) the residual covariance S _k is the following observation noise (matrix) R _k , partial differential matrix H _k in observation update, its transpose matrix H _k ^T, and the current time: It is obtained using the error covariance matrix P _{k | k−1} .

ここで、（ｒ₁，ｒ₂，ｒ₃）は、加速度センサ１４のデバイス評価の結果、予め求められた分散値である。

Here, (r ₁ , r ₂ , r ₃ ) is a dispersion value obtained in advance as a result of device evaluation of the acceleration sensor 14.

In the general expression of the extended Kalman filter, (4) the Kalman gain K _k is the error covariance matrix P _{k | k−1 at} the current time, the transposed matrix H _k ^T of the partial differential matrix in the observation update, and the residual It is obtained from the covariance inverse matrix S _k−1 . K _k is a 7 × 3 matrix and has real values as elements.

Similarly, (5) the updated state estimation value x _{k | k} and (6) the updated error covariance matrix P _{k | k} in the general expression of the extended Kalman filter are obtained using the variables obtained so far. Can be sought.

  The posture calculation unit 104 executes the observation update procedure in the extended Kalman filter using the above model and variable, compares the angle information in the horizontal direction with the horizontal angle information of the current quaternion vector, and calculates the error amount. Correct (roll pitch angle only).

((Second observation update procedure))
Next, a second observation update procedure in the extended Kalman filter will be described with reference to FIG. The posture calculation unit 104 performs the procedure using the yaw angle information calculated from the posture information acquired according to the TRIAD algorithm, and corrects the yaw angle component of the quaternion vector. A calculation method using the TRIAD algorithm will be described later.

FIG. 9 is similar to FIG. 8 in the general expression of the extended Kalman filter.
(1) Observation residual

に含まれる項目を説明する図である。図８の場合と同様に、１ステップ前の時間の観測値（ベクトル）ｈは、図９の式（１）−３のように表される。一方、図８の場合と異なり、観測値（ベクトル）ｚ_kは、図９の式（１）−２のように表される。

It is a figure explaining the item contained in. As in the case of FIG. 8, the observed value (vector) h at the time one step before is expressed as Expression (1) -3 in FIG. 9. On the other hand, unlike the case of FIG. 8, the observed value (vector) z _k is expressed as Expression (1) -2 in FIG. 9.

上記のベクトルは、ＴＲＩＡＤアルゴリズムによって求められた、ヨー角方向ベクトルである。

The above vector is a yaw angle direction vector obtained by the TRIAD algorithm.

Further, in the general expression of the extended Kalman filter, (2) the partial differential matrix (Jacobiane) H _k in the observation update obtains the partial differential of the observation value h at the time one step before, as in the first observation update procedure. Can be obtained.

Further, in the general expression of the extended Kalman filter, (3) the residual covariance S _k is the following observation noise (matrix) R _k , partial differential matrix H _k in observation update, its transpose matrix H _k ^T, and current time The error covariance matrix P _{k | k−1} is obtained.

ここで、（Ｔ₁，Ｔ₂，Ｔ₃）は、地磁気センサ１６のデバイス評価の結果、予め求められ
た分散値である。

Here, (T ₁ , T ₂ , T ₃ ) are dispersion values obtained in advance as a result of device evaluation of the geomagnetic sensor 16.

In the general expression of the extended Kalman filter, (4) Kalman gain K _k , (5) updated state estimated value x _{k | k} and (6) updated error covariance matrix P _{k | k} It can be obtained in the same way as the observation update procedure.

<< Calculation of posture information using TRIAD algorithm >>
Here, an example in which the posture calculation unit 104 calculates the second posture information using a known TRIAD algorithm will be described with reference to FIG.

  First, in step S10, initialization processing is executed at the time of factory shipment or setting by a user, and a vector representing a vertical downward component of acceleration and a reference vector of a geomagnetic vector are stored as AccRef and MagRef, respectively. Here, the vector representing the vertical downward component of the acceleration is obtained by conversion from the quaternion as described in step S20 below. The geomagnetic vector is a vector representing magnetic north input from the geomagnetic sensor. This step is executed only at the time of factory shipment or setting by the user. That is, the reference vector holds the same value unless the initialization process is executed.

  In step S30, MagFrameM, which is a 3 × 3 matrix, is calculated using AccFrame and MagFrame.

  In step S32, AccCrossMag is obtained by obtaining the outer product of AccFrame and MagFrame and normalizing the result.

  In step S34, AccCrossAcM is obtained by obtaining the outer product of AccFrame and AccCrossMag and normalizing the result.

  In step S36, AccFrame, AccCrossMag obtained in step S32, and AccCrossAcM obtained in step S34 (both are 1 × 3 matrices (vectors)) are combined to generate MagFrameM which is a 3 × 3 matrix.

  On the other hand, in step S40, MagRefM, which is a 3 × 3 matrix, is calculated using AccRef and MagRef.

  In step S42, MagCrossAcc is obtained by obtaining the outer product of AccRef and MagRef and normalizing the result.

  In step S44, MagCross is obtained by obtaining the outer product of AccRef and MagCrossAcc and normalizing the result.

  In step S46, AccRef, MagCrossAcc obtained in step S42, and MagCross obtained in step S44 (both 1 × 3 matrix (vector)) are combined to generate MagRefM, which is a 3 × 3 matrix.

  Here, the above step S40 (S42-S46) is executed when the initialization process is executed and AccRef and MagRef are changed, and thereafter, the value of MagRefM obtained until the initialization process is executed again. May be used while holding.

  In step S50, the inner product of MagFrame and MagRefM is obtained. The obtained matrix is called mag_triad (3 × 3). The matrix mag_triad is a matrix for converting from the device coordinate system to the absolute coordinate system. In the TRIAD algorithm, the three columns are called TRIAD1, TRIAD2, and TRIAD3, respectively.

  In step S60, an inverse matrix of mag_triad (matrix for conversion from the absolute coordinate system to the device coordinate system) is obtained and converted to a quaternion. The quaternion thus obtained is the second attitude information. The inverse matrix of mag_triad is the rotation matrix DCM1.

<Attitude information conversion processing unit 100>
The posture information conversion processing unit 100 in FIG. 4 will be described with reference to FIG. The posture information conversion processing unit 100 includes a posture information acquisition unit 105 and a coordinate system conversion processing unit 106. The posture information acquisition unit 105 acquires DCM1 from the posture information estimation unit 110 and acquires a yaw angle correction value from the correction parameter estimation processing unit 700.

  Then, the coordinate system conversion processing unit 106 multiplies the device coordinate system acceleration acquired by the device coordinate system triaxial acceleration acquisition unit 120 by the rotation matrix DCM1, and calculates the triaxial acceleration (absolute acceleration) in the absolute coordinate system. . Further, the coordinate transformation is performed on the triaxial acceleration vector in the absolute coordinate system by the rotation matrix DCM2.

  As shown in FIG. 11, the coordinate system conversion processing unit 106 uses the rotation matrix DCM1 to convert a triaxial acceleration vector in the device coordinate system into an acceleration vector in the absolute coordinate system.

  The rotation matrix DCM2 is a direction cosine matrix, and each element of the rotation matrix DCM2 is referred to as a direction cosine. The rotation matrix DCM2 is a matrix for coordinate conversion of a triaxial acceleration vector in the absolute coordinate system into an absolute coordinate system acceleration vector 107 obtained by correcting the traveling direction with a yaw angle correction value. The rotation matrices (DCM1 and DCM2) are the same 3 × 3 matrix.

  FIG. 12 is an example of a diagram illustrating the rotation matrix DCM2. The rotation matrix DCM2 is expressed as shown in FIG. 12 using Euler angles. As shown in FIG. 2, the rotation angle around the X axis is called roll angle φ, the rotation angle around the Y axis is called pitch angle θ, and the rotation angle around the Z axis is called yaw angle ψ.

  Since the yaw angle ψ is corrected by the yaw angle correction value, the coordinate system conversion processing unit 106 substitutes zero (= 0) for the roll angle φ and the pitch angle θ of the rotation matrix DCM2, and estimates the correction parameter for the yaw angle ψ. The yaw angle correction value (deg) notified from the processing unit 700 and acquired is substituted.

  Accordingly, the azimuth angle is corrected in the absolute coordinate system by the matrix operation using the rotation matrices DCM1 and DCM2 (the azimuth offset is canceled) and the absolute coordinate system acceleration vector 107 is moved in the traveling direction. It can be acquired by the estimation unit 200. A method for obtaining the yaw angle correction value will be described later with reference to FIG.

<Advancing direction estimation unit>
FIG. 13 is an example of a diagram illustrating the function of the traveling direction estimation unit 200. The traveling direction estimation unit 200 that executes the traveling direction estimation function in the inertial apparatus 1 includes a bandpass filter 201, a peak detection unit 204, a peak position storage unit 205, a movement acceleration storage unit 206, and horizontal component movement speed feature information. A management unit 207, a vertical component peak movement acceleration acquisition unit 208, a horizontal component movement speed feature information acquisition unit 209, a period acquisition unit 210, a determination unit 211, and a traveling direction calculation unit 212 are included. The traveling direction estimation unit 200 obtains the traveling direction for each step of the user from the absolute coordinate system acceleration vector 107 obtained by the posture information conversion processing unit 100.

  The band pass filter 201 removes a gravity component from the absolute coordinate system acceleration vector 107 output from the posture information conversion processing unit 100. The pass band is, for example, about 1-3 Hz, which is a general walking frequency. Note that the pass band can be appropriately changed according to the frequency related to the walking or movement of the measurement target of the inertial device 1. Here, the absolute acceleration from which the gravitational component is removed, which is output from the bandpass filter 201, is referred to as a movement acceleration 202. The movement acceleration 202 is stored in a movement acceleration storage unit 206, which will be described later. The vertical component movement acceleration 203, which is the vertical component of the movement acceleration, is passed to a peak detection unit 204, which will be described later.

  The peak detection unit 204 observes a change (time change) of the vertical component movement acceleration 203 in the movement acceleration 202 output from the bandpass filter 201, and determines a valley peak position (peak time or peak position) of the waveform. To detect. The detected peak position is stored in a peak position storage unit 205 described later. Hereinafter, a method for detecting the valley peak will be described.

  FIG. 14 is a waveform showing changes in the vertical component movement acceleration 203 (Z) and the horizontal component (X, Y) of the movement acceleration with the horizontal axis as time. In this way, each waveform has a waveform that has a period that matches the movement period (for example, the walking period). In particular, the vertical component movement acceleration 203 has a large amplitude compared to the horizontal component of about ± 1 m / s 2. A waveform is output. A mountain peak appears when a foot lands on the ground, and a valley peak appears at the moment when the other foot overtakes one axial foot.

  FIG. 15 is a diagram for explaining vertical motion characteristics in a walking motion. The walking movement is generally classified into a standing leg phase and a free leg phase according to the movement of the lower limbs. The stance phase (Stance Phase) is the period from when the heel of one side leg touches down to the point where the ipsilateral foot is released. In addition, the swing phase is a period from the time when the footpad of one lower limb leaves the ground until the footpad of the same side touches the ground. Furthermore, the walking motion is characterized by the presence or absence of a both-leg support period. In general, it is known that when walking is slow, the ratio of the both-leg support period to the entire walking increases and decreases as the walking speed increases. It is also known that the both-leg support period disappears when the running state is reached. Further, it is known that the amount of vertical movement (vertical direction) and the amount of horizontal movement (horizontal plane) of the body are maximized in the middle of the stance.

  The first half of the middle stance includes an action in which one kicked foot overtakes the axial leg (one kicked leg passes under the trunk). At this time, the body moves vertically upward, and vertical upward movement acceleration is generated. On the other hand, the second half of the middle stance includes the operation until the kicked foot of one foot comes into contact with the ground. At this time, the body moves vertically downward, and vertical downward movement acceleration is generated.

  FIG. 16 is a diagram for explaining the motion characteristics in the horizontal direction in the walking motion. The horizontal component of the movement acceleration in the first half of the middle stance is the influence of the acceleration when kicking one leg to move the trunk to the target point and the acceleration caused by the roll due to the movement of the center of gravity of the body including. On the other hand, the horizontal component of the movement acceleration in the latter half of the stance is the movement of the trunk accompanying the movement of the kicked foot to the heel contact point, the acceleration generated by the body roll in the direction of travel, and the heel. Includes the effect of acceleration due to vibration when touched. In other words, the latter half of the middle stance does not include the pure movement acceleration required for kicking.

  Therefore, in the present application, the traveling direction is estimated using the first half of the moving acceleration, which reflects the acceleration at the time of kicking one leg for the trunk movement.

  Therefore, paying attention to the vertical component movement acceleration 203, one step is measured by detecting a valley peak by signal threshold determination. The reason for detecting one step using the valley peak is that the horizontal movement acceleration at the mountain peak position where the foot lands may include vibration and noise accompanying the landing of the foot. The movement acceleration in the horizontal direction at the valley peak position reduces the influence accompanying the landing of the foot and more accurately reflects the actual acceleration accompanying walking.

  The peak detection unit 204 performs peak detection by capturing a moment when the vertical component movement acceleration 203 falls below a predetermined threshold Th and exceeds the threshold Th again. Here, an intermediate time between the time ta when the threshold value Th is exceeded and the time tb when the threshold value Th is exceeded can be set as the peak position. For example, the threshold value Th is desirably set to a value that is about half of the vertical movement acceleration when actually walking. For peak detection, any method other than the above may be used.

  Further, by recording the past peak position, the peak interval representing the time between the current peak position and the past peak position can be obtained.

  The peak position storage unit 205 stores the peak position detected by the peak detection unit 204. The peak position storage unit 205 stores the peak position (time) from the latest to the one corresponding to the past time region, for example, by a ring buffer. The peak position storage unit 205 stores at least the latest peak position and one past peak position, and is updated as needed according to the peak position obtained later. The number of stored peak positions may be changed as appropriate according to the storage capacity of the inertial device 1.

  The movement acceleration storage unit 206 stores the movement acceleration 202 output from the bandpass filter 201 as time-series data with the observed time information added thereto.

  In response to detection of the peak position by the peak detection unit 204, the horizontal component movement speed feature information management unit 207 converts the movement acceleration of the horizontal component within a predetermined period (τ) around the peak position to each component ( Integration processing is performed every X, Y) to calculate the horizontal velocity. This horizontal speed is referred to as horizontal component movement speed feature information. The horizontal component moving speed feature information is a vector indicating the relative value of the speed direction and the magnitude. The horizontal component moving speed feature information management unit 207 stores the horizontal component speed feature information together with the time information t. That is, the horizontal component movement speed feature information management unit 207 has a function as a calculation unit that calculates horizontal component movement speed feature information and a function as a storage unit that stores horizontal component movement speed feature information.

FIG. 17 shows waveforms corresponding to FIG. 14 and representing changes in the horizontal and vertical components of the movement acceleration with respect to time. In this example, the horizontal component movement acceleration during the period τ centered on the peak positions t1, t2, and t3 obtained from the waveform of the vertical component of the movement acceleration is time-integrated to obtain the horizontal component movement speed feature vector V _1. , V ₂ and V ₃ are calculated.

  The period (τ) is preferably a period within (tb−ta) time, for example. This is because, when integration is performed over the entire time range, the acceleration occurs due to the rolling of the body with respect to the traveling direction and the acceleration due to the vibration caused by the vibration when the heel is grounded. This is because the estimation may not be performed correctly.

  The horizontal component moving speed feature information is generated at the moment when one step is taken and one foot passes the shaft foot by the above-described peak detection processing and a series of processing thereafter. The generated feature information is a horizontal component moving speed feature vector V having features of size and direction. As illustrated in FIG. 18, the horizontal component moving speed feature vector represents the direction (traveling direction) and the strength in which the pedestrian's body is moving at the moment when one foot passes the axial foot.

  The vertical component peak movement acceleration acquisition unit 208 acquires the movement acceleration (vertical component peak movement acceleration) in the vertical component movement acceleration 203 corresponding to the peak position (time) at time t, and this acceleration is described later. 211.

  The horizontal component moving speed feature information acquisition unit 209 acquires the latest horizontal component moving speed feature information and past horizontal component moving speed feature information from the horizontal component moving speed feature information management unit 207, and acquires the acquired horizontal component moving speed feature information. The information is passed to the determination unit 211 described later.

  The period acquisition unit 210 acquires a plurality of peak positions from the peak position storage unit 205, and executes a conversion process into a movement period (for example, a walking period) of a measurement target. Moreover, the period acquisition part 210 can acquire the newest movement period and the past movement period by calculating the difference of a some peak position sequentially. The period acquisition unit 210 passes the acquired plurality of movement periods to the determination unit 211 described later.

  The determination unit 211 determines whether or not the various types of information detected so far are data acquired from the walking motion by executing the procedure shown in FIG. The walking motion is a moving motion depending on the measurement object including walking and running. On the other hand, non-walking motion that is not walking motion, such as when the inertial device is randomly or intentionally shaken or when acceleration is received from the external environment (for example, when it is moved by an external moving object) This is an operation not caused only by the moving operation by the measurement object. Hereinafter, it demonstrates along the step shown by FIG.

  First, in step S100, when the vertical component peak movement acceleration acquired from the vertical component peak movement acceleration acquisition unit 208 is within a predetermined range, the process proceeds to step S200. When that is not right, it progresses to step S600 and the determination part 211 determines with originating in a non-walking motion. The predetermined range for the vertical component peak movement acceleration is set in advance by the manufacturer or user of the inertial device 1 according to the characteristics of the positioning target of the inertial device 1 (for example, the walking characteristics of the pedestrian).

  Next, when the size of the horizontal component moving speed feature information (vector) acquired from the horizontal component moving speed feature information management unit 207 is within a predetermined range in step S200, the process proceeds to step S300. When that is not right, it progresses to step S600 and the determination part 211 determines with originating in a non-walking motion. In addition, the predetermined range about the magnitude | size of horizontal component moving speed characteristic information (vector) is the manufacturer or user of the inertial apparatus 1 according to the characteristic (for example, walking characteristic of a pedestrian) of the positioning object of the inertial apparatus 1. Is set in advance.

  Next, in step S300, when the movement period acquired from the period acquisition unit 210 is within a predetermined range, the process proceeds to step S400. When that is not right, it progresses to step S600 and the determination part 211 determines with originating in a non-walking motion. In addition, the predetermined range about a movement period is preset by the manufacturer or user of the inertial apparatus 1 according to the characteristic of the positioning target of the inertial apparatus 1 (for example, the walking characteristic of a pedestrian).

  Next, in step S400, when the fluctuation width of the moving speed in the left-right direction is within a predetermined range, the process proceeds to step S500, and the determination unit 211 determines that it is caused by the walking motion. When that is not right, it progresses to step S600 and the determination part 211 determines with originating in a non-walking motion.

  Here, the fluctuation width of the left and right moving speed will be described with reference to FIG. In human walking, when the right foot is stepped on, a moving speed vector is generated in the right direction, and when the left foot is stepped on, a moving speed vector is generated in the left direction. In order to make this determination, it is determined whether or not the horizontal component moving speed feature vector matches this characteristic.

  First, the determination unit 211 combines the start point and end point of the horizontal component moving speed feature vector as shown in FIG. Next, the determination unit 211 calculates the distance dn between the straight line connecting the midpoints of the respective Vn vectors and the end point of each Vn vector. Then, the determination unit 211 determines whether dn is within a predetermined range, and if it is within the range, determines that it is caused by walking motion. The predetermined range for dn is set in advance by the manufacturer or user of the inertial device 1 according to the characteristics of the positioning target of the inertial device 1 (for example, the walking characteristics of the pedestrian).

  In step S500, the determination unit 211 determines that it is caused by a walking motion.

  In step S600, the determination unit 211 determines that the non-walking motion is caused.

  Of the determinations in steps S100 to S400 described above, some determinations may be omitted. However, when all the determinations are made, it is possible to estimate the traveling direction with higher accuracy.

  The advancing direction calculation unit 212 performs the following processing in order to obtain the advancing direction 213 for each step when the above-described determination unit 211 determines that “it is due to walking motion”.

When the user moves from the zeroth step to the first step, the traveling direction calculation unit 212 acquires the horizontal component movement speed feature vector V ₀ from the horizontal component movement speed feature information acquisition unit 209 (FIG. 21A). Further, when the user moves from the first step to the second step, the traveling direction calculation unit 212 acquires the horizontal component movement speed feature vector V ₁ from the horizontal component movement speed feature information acquisition unit 209 (FIG. 21A). ).

Here, the traveling direction calculation unit 212, normalized and vector V ₀ and the vector V _1, to obtain a 'vector V ₁ and' vector V ₀ (FIG. 21 (b)). Then, the traveling direction calculation unit 212 obtains the combined vector (V ₀ ′ + V ₁ ′) and estimates the direction of the combined vector as the traveling direction 213 for each step ((FIG. 21C)). The above process is sequentially executed every time the user takes one step.

  As described above, the inertial apparatus 1 calculates the acceleration in the absolute coordinate system using the vertical peak valley position of the acceleration in the absolute coordinate system and using a certain time frame before and after the acceleration in order to estimate the traveling direction for each step. A horizontal velocity vector is used. Thereby, the influence by the vibration etc. which arise in the case of a pedestrian's landing can be decreased, and the precision of estimation improves.

  Further, the inertial device 1 evaluates whether or not the sensor information from the geomagnetic sensor is reliable. If the sensor information is reliable, the inertial device 1 corrects a vector representing the attitude of the inertial device 1 (yaw angle component). Use for. As a result, the vector representing the posture can be corrected with higher accuracy using the geomagnetic sensor information.

<Position estimation unit>
FIG. 22 is an example of a diagram illustrating functions of the position estimation unit 800. The position estimation unit 800 estimates the current position. The position estimation unit 800 includes a traveling speed estimation unit 300, an absolute position information input unit 400, an absolute position estimation unit 500, a speed correction unit 350, and a map matching unit 600. Hereinafter, functions provided by the traveling speed estimation unit 300, the absolute position information input unit 400, the absolute position estimation unit 500, the map matching unit 600, and the speed correction unit 350 will be described.

<< Progression speed estimation part >>
FIG. 23 shows a detailed functional block diagram inside the traveling speed estimation unit 300. The travel speed estimation unit 300 includes a horizontal component speed feature information acquisition unit 301, a vertical component peak acceleration acquisition unit 302, a second period acquisition unit 303, a blur width acquisition unit 304, a travel direction acquisition unit 305, and a conversion unit. 306, a traveling speed waveform generation unit 312, a traveling speed waveform synthesis unit 314, and a traveling speed waveform storage unit 315.

  The travel speed estimation unit 300 calculates the actual travel speed of the measurement target based on the horizontal component speed feature information 214, the vertical component peak acceleration 215, the period 216, the blur width 217, and the travel direction 213 calculated by the travel direction estimation unit 200. A traveling speed estimation vector is calculated. The processing contents will be described below.

  The horizontal component velocity feature information acquisition unit 301 acquires horizontal component velocity feature information 214 (horizontal component velocity feature vector) from the horizontal component movement velocity feature information management unit 207 and inputs this information to the conversion unit 306.

  The vertical component peak acceleration acquisition unit 302 acquires the vertical component peak acceleration 215 from the movement acceleration storage unit 206 and inputs this information to the conversion unit 306.

  The second cycle acquisition unit 303 uses the information stored in the peak position storage unit 205 to acquire a cycle 216 related to the movement of the measurement target, and inputs this information to the conversion unit 306.

  The blur width acquisition unit 304 uses the horizontal component velocity characteristic information 214 to acquire the blur width of the left and right moving speeds (hereinafter referred to as the blur width 217), and inputs this information to the conversion unit 306. The method of calculating the blur width 217 is as described with reference to FIG.

  The traveling direction acquisition unit 305 acquires the traveling direction 213 output from the traveling direction calculation unit 212 of the traveling direction estimation unit 200 and inputs this information to the conversion unit 306.

  The conversion unit 306 converts the input horizontal component velocity characteristic information 214, vertical component peak acceleration 215, cycle 216, blur width 217, and traveling direction 213 into a velocity parameter Pa307, an intensity parameter Pb308, a cycle parameter Pc309, and a blur width parameter, respectively. It converts into Pd310 and the advancing direction parameter Pe311. In this conversion, for example, each piece of information 214 to 217 is normalized according to a predetermined rule. The conversion unit 306 can use any method in order to convert various input information into a predetermined range of parameters.

  The progress velocity waveform generation unit 312 refers to the parameter DB 313 (for example, FIG. 23) using each input parameter 307-311 and the attribute information of the measurement target registered in advance as a key, and the corresponding velocity generation coefficient Ca. -Get Cc. Next, the traveling speed waveform generation unit 312 generates a traveling speed waveform according to an expression described later using each input parameter and the acquired speed generation coefficient Ca-Cc.

  Here, the measurement target attribute information includes, for example, the user's sex, age, height, and the like. The attribute information of the measurement target may be arbitrary information for specifying the measurement target. Examples of other attribute information include, but are not limited to, the type of measurement target (human, animal, biped robot, etc.), identification number, model number, property (high speed, low speed), and the like.

  The parameter DB 313 illustrated in FIG. 24 associates gender, age, and height, which are attribute information of the measurement target, with the parameters 307 to 310 and the speed generation coefficient Ca-Cc. In the example of FIG. 24, for the sake of simplicity, the traveling direction parameter Pe311 is not used to obtain the speed generation coefficient Ca-Cc (that is, the speed generation coefficient is determined regardless of the traveling direction). However, the parameter DB 313 may be constructed using the traveling direction parameter Pe311.

  The parameter DB 313 allows the provider of the inertial apparatus 1 to perform the operation specified by the parameters 307 to 311 on the group to be measured specified by the attribute information, and based on the acquired data, It is constructed in advance. For the speed parameter Pa, the norm value is registered in the table.

  When there is no entry in the parameter DB 313 that matches the input various parameters, the progress velocity waveform generation unit 312 acquires a velocity generation coefficient Ca-Cc of an entry similar to the combination of parameters. For example, in order to detect a similar entry, the traveling speed waveform generation unit 312 can select the one having the smallest total value of root mean square of each parameter. Any method can be used to detect similar entries. The traveling speed waveform generation unit 312 passes the acquired speed generation coefficient Ca-Cc to the traveling speed waveform generation unit 312.

  The traveling speed waveform generation unit 312 uses the speed generation coefficient Ca-Cc and each of the input parameters Pa307-Pe311 to calculate the speed of the measurement target at time 0 ≦ t ≦ Pc, for example, using the following equation: A progress velocity waveform representing the change is generated.

なお、進行速度波形生成部３１２は、上記の式以外の任意の式を用いて、進行速度波形を生成しても良い。

The traveling speed waveform generation unit 312 may generate the traveling speed waveform using an arbitrary expression other than the above expression.

  An example of the generated progress velocity waveform is shown in FIG. Here, θ corresponds to the traveling direction parameter Pe. The equation represented by Equation 11 includes an estimation term (first term) based on the velocity parameter Pa307, an estimation term (second term) based on the intensity parameter Pb308, and an estimation term (third term) based on the blur width parameter Pd310. . Each term is multiplied by a speed generation coefficient Ca-Cc as a weight, and it is possible to generate a walking speed waveform with high accuracy by performing speed estimation based on a plurality of parameters. The speed generation coefficient Ca-Cc is normalized in advance so that the sum is 1, and is stored in the parameter DB 313. The travel speed waveform generation unit 312 passes the generated travel speed waveform to the travel speed waveform synthesis unit 314.

  When the progress velocity waveform synthesizer 314 acquires the progression velocity waveform from the progression velocity waveform generator 312, the progression velocity waveform synthesis unit 314 obtains a progression velocity waveform synthesized in the past stored in the progression velocity waveform storage unit 315, which will be described later. Synthesize. At this time, the traveling speed waveform synthesizing unit 314 synthesizes these traveling speed waveforms by integrating them at the corresponding time positions. The progress velocity waveform synthesizing unit 314 may synthesize the progression velocity waveform by an arbitrary method (for example, when a plurality of progression velocity waveforms overlap each other, adopt each maximum value).

  FIG. 26 illustrates an example in which the traveling speed waveform synthesis unit 314 integrates and synthesizes a plurality of traveling speed waveforms. The travel speed waveform synthesis unit 314 stores the synthesized travel speed waveform in the travel speed waveform storage unit 315.

  The traveling speed waveform storage unit 315 stores the traveling speed waveform synthesized by the traveling speed waveform synthesis unit 314 together with time information.

  In this way, the latest traveling speed waveform synthesized based on the parameters acquired as needed is stored in the traveling speed waveform storage unit 315. The absolute position estimation unit 500 and the map matching unit 600, which will be described later, can acquire travel speed information representing the latest travel speed by referring to the value of the travel speed waveform synthesized at the present time. As described above, since the traveling speed information is represented by two components in the horizontal direction, this traveling speed information is hereinafter referred to as a traveling speed estimation vector 316.

<< Absolute position information input section >>
FIG. 27 shows a detailed functional block diagram of each of the absolute position information input unit 400, the absolute position estimation unit 500, and the map matching unit 600.

  The absolute position information input unit 400 includes a first absolute position acquisition unit 401, a second absolute position acquisition unit 402, a positioning time measurement unit 403, and an error correction unit 404. The absolute position information input unit 400 inputs information indicating the absolute position of the inertial device 1 and error information indicating the magnitude of the error included in the information to the absolute position estimation unit 500.

The first absolute position acquisition unit 401 communicates with a position information transmission device provided outside by, for example, Bluetooth (registered trademark) communication, and acquires absolute position information indicating the absolute position of the inertial device 1. The absolute position information may be, for example, position vectors X1, Y1, and Z1 represented by latitude, longitude, and altitude, or may be an arbitrary vector representing a position with a fixed point as a base point. The first absolute position acquisition unit 401 also acquires error information σ ₁ representing an error in the information from the position information transmission device. The error information σ ₁ is an error covariance matrix for the absolute position acquired from the position information transmitting apparatus. The error information σ ₁ includes, for example, an error value that is determined according to the radio wave intensity related to the communication between the position information transmission device and the inertial device 1. For example, when the radio field intensity indicates a low value, an error covariance matrix having a larger error value is acquired. The error information may be stored in the inertial device 1 in advance or may be transmitted from the position information transmitting device. The acquired position vector and error information are passed to a first observation update processing unit 502 of the absolute position estimation unit 500 described later.

The second absolute position acquisition unit 402 uses the communication means (for example, GPS or IMES (Indoor Messaging System)) different from the first absolute position acquisition unit 401 and the absolute position information X ₂ , Y ₂ , Z ₂ and error information. Get σ ₂ . In addition, the absolute position acquisition part should just exist at least one, and the number is arbitrary. The acquired position vector and error information are passed to a second observation update processing unit 503 of the absolute position estimation unit 500 described later.

  The absolute position information and error information acquired by the first absolute position acquisition unit 401 and the second absolute position acquisition unit 402 are shown as absolute position information 405 and error information 406 in FIG.

  The positioning time measurement unit 403 measures the intervals at which the first absolute position acquisition unit 401 and the second absolute position acquisition unit 402 respectively acquire absolute position information, and passes the intervals to an error correction unit 404 described later. .

The error correction unit 404 determines whether or not the interval received from the positioning time measurement unit 403 is a predetermined interval, and the error covariance matrix output from each absolute position acquisition unit according to the size of the interval Correction is performed so that each variance value of (σ ₁ or σ ₂ ) becomes large. For this purpose, the error correction unit 404 can use a table that is prepared in advance and associates the interval [seconds] with the correction amount (value to be multiplied by the variance value). Alternatively, the error correction unit 404 may provide a threshold value for the size of the interval, and may apply a predetermined correction to the error covariance matrix when the threshold value is exceeded.

  In particular, since error information of GPS and IMES is not generated in consideration of the influence of multipath, the reliability of the error information may change depending on the radio wave condition. There is a correlation as shown in FIG. 28 between the S / N ratio representing the ratio between the signal amount and the noise amount and the size of the positioning interval. Therefore, the error correction unit 404 can absorb the change in the reliability of the error information by performing correction so that the positioning error becomes larger when the positioning interval exceeds a predetermined interval. The case where error information is not transmitted directly will be described later.

  The external position information transmitting apparatus may transmit the absolute position information and the error information by means such as infrared, wireless LAN, visible light communication, positioning means using a camera, etc. in addition to the communication method described above. The inertial device 1 can receive absolute position information and error information by corresponding receiving means. The received absolute position information and error information are input to the observation update processing unit of the absolute position estimation unit 500, similarly to the absolute position acquisition unit described above. The number of absolute position acquisition units and observation update processing units is arbitrary.

<< Absolute position estimation part >>
The absolute position estimation unit 500 includes a time evolution processing unit 501, a first observation update processing unit 502, a second observation update processing unit 503, and a third observation update processing unit 504.

The absolute position estimation unit 500 uses the traveling speed estimation vector 316 and the traveling speed measurement error information 317 provided together with the traveling speed estimation vector 316 to use the current position and error information indicating the error (current position and error information). 505). The traveling speed measurement error information 317 is an error covariance matrix σ _v representing an error of the traveling speed estimation vector 316, and fixed data obtained by system identification is used. Alternatively, the traveling speed measurement error information 317 may be a plurality of data prepared according to the traveling speed.

  The absolute position estimation unit 500 updates the current position and error information 505 using the absolute position information (position vector) and error information (error covariance matrix) output from the absolute position information input unit 400. Further, the absolute position estimation unit 500 updates the current position and error information 505 using the position information (position vector) and error information (error covariance matrix) output from the map matching unit 600.

  The current position and error information 505 are calculated or updated using an extended Kalman filter. Here, one time development procedure (time development processing unit 501) and three observation update procedures (first observation update processing unit 502, second observation update processing unit 503, third observation update processing unit) 504) are executed in parallel. The variables and models used in each procedure are described below.

  The time evolution processing unit 501 executes the procedure for time evolution of the extended Kalman filter according to the variable and model definitions shown in FIGS. 29 to 31, and calculates or updates the current position of the inertial device 1 and the error information 505. Here, variables and models in the extended Kalman filter are defined as shown in FIG. That is, the state estimated value at the current time is defined by a three-dimensional position vector as shown in Expression (1) -1 in FIG. Further, the input amount is defined by a traveling speed estimation vector 316 output by the traveling speed estimation unit 300 as shown in Expression (1) -4 in FIG. Further, the state estimation model of the system is defined as Expression (1) -4 in FIG.

  Further, as shown in FIG. 30, in the partial differentiation matrix (Jacobiane) in time evolution, the partial differentiation on the right side in the system state estimation model becomes the partial differentiation matrix as it is.

Further, as shown in FIG. 31, the process noise Q _k is the traveling speed measurement error information 317 (σ _v ) obtained in advance by system identification, and is a constant calculated in advance. The error covariance matrices P _{k | k-1} and P _{k-1 | k-1 at} the current time are 3 × 3 matrices, and the elements in the matrix are real numbers.

The first observation update processing unit 502 executes the observation update procedure of the extended Kalman filter, and calculates or updates the current position of the inertial device 1 and the error information 505. Here, variables in the extended Kalman filter will be described with reference to FIG. That is, the observed value of the time one step before becomes a three-dimensional position vector as shown in Expression (1) -3 in FIG. In addition, the observed value is a position vector (absolute position information) output from the first absolute position acquisition unit 401, as in Expression (1) -2 in FIG. Observation residual by h and z _k above

が求まる。

Is obtained.

In addition, the partial differential matrix (Jacobiane) H _k in (2) observation update in the general expression of the extended Kalman filter is obtained by obtaining the partial differential of the observation value h represented by the expression (1) -3 in FIG.

Further, (3) the residual covariance S _k in the general expression of the extended Kalman filter is the observation noise (matrix) R _k that is the error information output from the first absolute position acquisition unit 401 and the partial differential matrix in the observation update. H _k , its transposed matrix H _k T and the current time error covariance matrix P _{k | k−1} are used.

なお、ｒ₁はＸ軸、ｒ₂はＹ軸、ｒ₃はＺ軸の成分を表す。

R ₁ represents the X axis, r ₂ represents the Y axis, and r ₃ represents the Z axis.

Further, in the general expression of the extended Kalman filter, (4) the Kalman gain Kk is the error covariance matrix P _{k | k−1 at} the current time, the transposed matrix HkT of the partial differential matrix in the observation update, and the covariance of the residual It is obtained from the inverse matrix Sk-1.

Similarly, (5) the updated state estimation value x _{k | k} and (6) the updated error covariance matrix P _{k | k} in the general expression of the extended Kalman filter are obtained using the variables obtained so far. Can be sought.

Similar to the first observation update processing unit 502, the second observation update processing unit 503 executes the observation update procedure of the extended Kalman filter, and calculates or updates the current position of the inertial device 1 and the error information 505. The variables in the extended Kalman filter are the same as the first observation update processing unit 502 except that the observation value z _k and the observation noise R _k are the position vector and error information output by the second absolute position acquisition unit 402. It is the same.

Similar to the first observation update processing unit 502, the third observation update processing unit 504 executes an extended Kalman filter observation update procedure to calculate or update the current position of the inertial device 1 and the error information 505. Variables in the extended Kalman filter are the same as those in the first observation update processing unit 502 except that the observation value z _k and the observation noise R _k are position vectors and error information output by the map matching unit 600, which will be described later. It is.

  As described above, the absolute position estimation unit 500 can estimate the current position with high accuracy by sequentially updating the current position and the error information 505 in the framework of the extended Kalman filter. An external application installed in the inertial device 1 refers to the current position and the error information 505 to acquire not only the estimated position of the current position but also the direction of the inertial device (heading information, estimated yaw angle). Can do.

  When the first observation update processing unit 502 or the second observation update processing unit 503 calculates or updates the current position of the inertial apparatus 1 and the error information 505, the absolute position correction flag 319 becomes TRUE. The absolute position correction flag 319 that becomes TRUE is notified to the correction parameter estimation processing unit 700, and thus becomes FALSE (cleared before the absolute position / error information of the next step is notified).

<< Map matching >>
The map matching unit 600 includes a map matching processing unit 601, acquires the latest current position, error information 505, and the traveling speed estimation vector 316, and executes map matching processing.

  The map matching unit 600 includes a map matching processing unit 601, acquires the latest current position, error information 505, and the traveling speed estimation vector 316, and executes map matching processing.

The map matching processing unit 601 acquires the latest current position, error information 505, and a traveling speed estimation vector 316, refers to a map database 602 prepared in advance, and acquires region information representing a region that can proceed. Next, the map matching processing unit 601 applies a particle filter algorithm that is a known technique (for example, Non-Patent Document 3), and executes map matching processing. The map matching processing unit 601 corrects the current position to a travelable area when the current position indicates a place where the travel is impossible on the map. Further, the map matching processing unit 601 calculates an error covariance matrix σ _m representing the error amount of each component of the position vector representing the corrected position by internal processing. The map matching processing unit 601 uses the position vector (X _m , Y _m , Z _m ) representing the corrected current position and the error covariance matrix σ _m as described above for the third observation of the absolute position estimation unit 500. The data is transferred to the update processing unit 504.

<Speed correction unit>
The speed correction unit 350 executes a process of correcting the traveling speed estimation vector 316 based on the walking speed estimation parameter notified from the correction parameter estimation processing unit 700. The correction method may be, for example, multiplying the traveling speed estimation vector 316 by the correction speed estimation parameter. Alternatively, the correction speed estimation parameter and the correction value may be associated with the lookup table, the correction value associated with the correction speed estimation parameter may be acquired from the lookup table and multiplied by the traveling speed estimation vector 316. In this case, nonlinear correction is possible. The correction method is not limited to these.

  FIG. 33 schematically shows speed information after correction. The speed information after waveform synthesis is corrected by the corrected speed estimation parameter, and corrected speed information 316A is obtained. In FIG. 33, the speed information is corrected to be large, but the speed information may be corrected to be small.

<Positioning error / coordinate estimation unit>
The positioning error / coordinate estimation unit 330 executes processing for estimating error information with respect to the absolute position coordinates. For example, the inertial device 1 can receive the absolute position information by radio waves such as the Bluetooth (registered trademark) communication module 20, but the absolute position information indicates the position of the transmitter 340 that transmits the absolute position information. The position of 1 is not shown. However, in this case, the error can be estimated from the magnitude of the received signal strength.

表１は図１で受信エリアＡＲＥＡ１と受信エリアＡＲＥＡ２を形成する送信機３４０が送信した絶対位置情報を示す。絶対位置情報ＩＤに対応付けて緯度と経度が周囲の慣性装置１に送信されている。慣性装置１はこの絶対位置情報を受信した際の受信信号強度を測定している。測位誤差・座標推定部３３０はこの受信信号強度を用いることで誤差情報を推定できる。

Table 1 shows the absolute position information transmitted by the transmitter 340 forming the reception area AREA1 and the reception area AREA2 in FIG. The latitude and longitude are transmitted to the surrounding inertial apparatus 1 in association with the absolute position information ID. The inertial device 1 measures the received signal strength when the absolute position information is received. The positioning error / coordinate estimation unit 330 can estimate error information by using the received signal strength.

  FIG. 34 is a diagram illustrating an example of a change in received signal strength according to the distance between the transmitter 340 for absolute position information and the inertial device 1. If such a waveform is known, the positioning error / coordinate estimation unit 330 can estimate the distance to the transmitter 340 based on the received signal strength at which the inertial apparatus 1 receives the absolute position information. Specifically, the installer or the like makes a function or table for converting the received signal strength into distance, and the positioning error / coordinate estimation unit 330 estimates the distance obtained from these as error information. The error information obtained in this way is used as error information 406 in FIG.

  When the transmitter 340 transmits absolute position information using sound waves (sound or non-audible sound), the error can be estimated based on the time until the sound or non-audible sound arrives or the received sound volume.

  Further, the transmitter 340 may transmit only the absolute position information ID, not the absolute position information itself. In this case, the inertial device 1 acquires absolute position information with reference to a correspondence table of absolute position information ID and absolute position information prepared in advance.

<Correction parameter estimation processing unit 700>
The correction parameter calculation unit 701 illustrated in FIG. 4 includes a correction parameter calculation unit 701, a movement state detection unit 702, and a positioning history storage unit 703. First, the positioning history storage unit 703 stores the absolute position and error information notified from the position estimation unit 800 in association with time information. The positioning history storage unit 703 is constructed in the RAM 12 or ROM 13.

  The positioning history storage unit 703 stores the absolute position corrected by the first observation update processing unit 502 or the second observation update processing unit 503 by a parametric method. The position acquired by the time evolution processing unit 501 by inertial navigation is also stored. When the following movement state determination is not performed, only the corrected absolute position may be stored.

  The movement state detection unit 702 detects whether the movement detected by the inertial device 1 is due to walking or running of a person or due to absolute position correction. Specifically, an absolute position from when the absolute position correction flag TRUE is detected until the next time the absolute position correction flag becomes TRUE is stored in the positioning history storage unit 703. Since the change in absolute position detected from when the absolute position correction flag becomes TRUE to the next time when the absolute position correction flag becomes TRUE is the movement distance obtained only by inertial navigation, if this is a predetermined distance or more It can be determined that the movement is due to walking or running. This predetermined distance is appropriately determined. For example, it may be several meters to several tens of meters, but is not limited to this.

  The processing of the correction parameter calculation unit 701 will be described with reference to FIG. Finally, when the absolute position correction flag becomes TRUE, the absolute position of the inertial apparatus 1 corrected by the parametric method using the absolute position information is P′1. The absolute position, the position 1001, and P′1 constituting the locus 1012 are stored in the positioning history storage unit 703. Further, when the absolute position of the inertial device 1 is corrected by a parametric method using the absolute position information, P′2 is also stored. An absolute position correction flag (TRUE) is assigned to P′1 and P′2.

  Next, before the absolute position correction flag becomes TRUE, the inertial apparatus 1 is at the position 1001 estimated only by inertial navigation. On the other hand, the position 1001 is corrected to the position P′2 by the absolute position information received in the reception area AREA2.

  Accordingly, the distance and the direction are shifted in the process of the inertial device 1 moving from the reception area AREA1 to the reception area AREA2. The correction parameter calculation unit 701 creates a correction parameter for correcting this deviation. Specifically, the distance B between P′1 and the position 1001 and the distance A between P′1 and P′2 are calculated. Since the position calculated by inertial navigation tends to be larger by “distance A / distance B”, “distance A / distance B” is set as a walking speed estimation parameter.

  Further, since the difference between the direction connecting P′1 and P′2 and the direction connecting P′1 and the position 1001 is an error with respect to the correct direction of the inertial device 1, this difference can be set as the yaw angle correction value Δψ. . Even if the direction is not the direction connecting P′1 and P′2 or the position 1001, the yaw angle correction is performed even if the position after the pedestrian walks slightly from P′1 and the direction connecting the P′2 or the position 1001 is used. The value Δψ can be obtained. Therefore, P′1 itself may not be the starting point for calculating the azimuth, and the absolute position in the past stored in the positioning history storage unit 703 may be used. However, the absolute position after the absolute position correction flag (TRUE) is assigned is preferably closer to the absolute position to which the absolute position correction flag (TRUE) is assigned.

  The correction parameter calculation unit 701 corrects the direction difference according to the magnitude of the error information calculated when the position is corrected by a parametric method, instead of directly using the above-described direction difference as the yaw angle correction value Δψ. May be. For example, the error information is divided by a predetermined threshold value, and if the result is 1 or less, the direction difference is determined as it is as the yaw angle correction value Δψ. When the division result is larger than 1, a value obtained by multiplying the division result by the direction difference is determined as the yaw angle correction value Δψ. Thereby, when the error information is large, the traveling direction can be largely corrected.

  As described above, since the posture information conversion processing unit 100 corrects the direction (yaw angle) of the inertial device 1 with the yaw angle correction value, the direction of the inertial device 1 is corrected to the correct direction every time the absolute position information is acquired. . Moreover, since the position estimation unit 800 corrects the traveling speed estimation vector using the walking speed estimation parameter, the absolute position detected by inertial navigation can be corrected appropriately.

<Overall operation of inertial device>
FIG. 35 is an example of a flowchart showing a procedure for estimating walking speed estimation parameters and yaw angle correction values.

  The position estimation unit 800 estimates the walking speed and current position of the person by inertial navigation using the traveling direction 213 estimated by the traveling direction estimation unit 200 (S10).

  Next, when the absolute position information input unit 400 acquires the absolute position information from the transmitter 340 by the inertial apparatus 1 entering the reception area (Yes in S20), the position estimation unit 800 uses a parametric method to determine the current position. It correct | amends and the error information corresponding to a position coordinate and this position coordinate is estimated (S30).

  Next, the position estimation unit 800 sets the position coordinates and error information corrected using the absolute position information to the current position by inertial navigation (S40). That is, the initial position for the inertial navigation to accumulate the position coordinates based on the walking speed is reset to the position coordinates and error information corrected using the absolute position information. Note that an absolute position correction flag of TRUE is transmitted to the correction parameter estimation processing unit 700.

  Next, the correction parameter calculation unit 701 determines whether the correction condition is satisfied (S50). The correction condition indicates that the position coordinate history based only on inertial navigation indicates that the robot has walked between any two points over a predetermined distance, and correction of the position coordinates obtained by a parametric method using absolute position information. The later error information is below a certain threshold. It can be determined that the position is not corrected by receiving the absolute position information by the former (the movement detected by the inertial device 1 can be determined to be due to walking or running of a person), and the positioning result by inertial navigation by the latter or It can be determined that there is some error in the absolute position information. Note that although it is possible to create a correction parameter in a preferable situation by determining both the former and the latter, only one of the former and the latter may be determined, or neither may be determined.

  When the determination in step S50 is Yes, the correction parameter calculation unit 701 calculates the coordinate difference (distance) and direction between the two points of the latest corrected position coordinates and the corrected position coordinates last time ( S60).

  Next, the correction parameter calculation unit 701 calculates a coordinate difference (distance) obtained only by inertial navigation and a direction between two points (S70). The starting point of the coordinates of two points obtained only by inertial navigation is the value reset as described above.

  Then, the correction parameter calculation unit 701 determines the result of dividing the coordinate difference in step S60 by the coordinate difference in S70 as a walking speed correction parameter and notifies the speed correction unit 350 of the position estimation unit 800 (S80).

  Further, the correction parameter calculation unit 701 feeds back the difference between the direction of step S60 and the direction of S70 to the posture information conversion processing unit 100 as a yaw angle correction value (S90). Thereafter, steps S10 to S90 are repeatedly executed.

<Positioning result>
FIG. 36 is an example of a diagram illustrating a positioning result obtained when moving in five reception areas and a state of correction. P1 to P5 are absolute position information transmitted by the transmitter 340, and P'1 to P'5 are positions corrected by a parametric method using the absolute position information. Positions 1002 to 1005 are positions estimated only by inertial navigation. AREA 1 to 5 are ranges in which absolute position information from the transmitter 340 can be received. E1 to E5 are error information calculated when the position is corrected by a parametric method.

  In the reception area AREA2, the position 1002 and the position P2 measured by the inertial navigation are greatly shifted, but are corrected to the position P′2 by the absolute position information. Thereafter, in the reception areas AREA3 to 5, the positions 1003 to 1005 measured by inertial navigation are not greatly shifted from the positions P′3 to 5, but it is understood that the positions 1002 to 1005 are corrected in each reception area.

表２は、測定機が出力する絶対位置情報と、パラメトリックな手法で補正された位置とを数値で示す。

Table 2 shows the absolute position information output by the measuring machine and the position corrected by the parametric method by numerical values.

  As can be seen from FIG. 36 and Table 2, by applying a parametric method, the absolute position is updated by adding error information when the absolute position information is received. For this reason, when the error is extremely small, the position to be corrected is close to the value of the position coordinate indicated by the absolute position information of P1 to P5, and when the error is large, it is outside by the error included around the position coordinate. The current position is updated to the position offset at.

  This is because by applying a parametric method, the reliability (error covariance matrix) for converging the estimated value based on the magnitude of the error is determined, and the position coordinates are interpolated before and after correction. Therefore, by applying a parametric method, more accurate position estimation can be performed as compared with a correction method for matching the received position coordinates when absolute position information is received.

  If absolute position information is received and steps S50 to S90 in FIG. 35 are executed based on a correction method for matching the received position coordinates themselves, the steps are executed even if the absolute position information has a large error. End up. For this reason, the estimation accuracy of the walking speed correction parameter and the yaw angle correction value decreases. Or wrong estimation will be performed.

  In the present embodiment, a parametric technique is applied and then steps S50 to S90 are executed, so that it is possible to estimate walking speed correction parameters and yaw angle correction values with higher accuracy.

  In this embodiment, since the distance is obtained from the coordinates of the two points, the walking speed correction parameter is not affected by how the pedestrian walks between the two points. Patent Document 2 discloses a method of learning by associating a walking tempo with a parameter indicating a user's stride or moving speed, using a user's absolute position as a trigger and moving a predetermined distance. However, with this method, when walking so as to meander and connect two points at the absolute position, the distance relationship is broken because it is based on the number of steps counted and the walking tempo, and a correct learning result cannot be obtained.

  From the above, inertial navigation by both the correction by estimating and feeding back the walking speed correction parameter and yaw angle correction value, and the improvement of the estimation accuracy of the walking speed correction parameter and yaw angle correction value by applying the parametric method The position estimation accuracy at can be improved.

  FIG. 37 is an example of a diagram for comparing the movement trajectories of pedestrians obtained by the conventional technique and the technique of the present embodiment. FIG. 37A shows a movement locus according to the conventional technique, and FIG. 37B shows a movement locus according to the technique of the present embodiment. In FIG. 37A, the deviation of the azimuth angle and the estimation error of the walking speed are superimposed on the current position estimated by the inertial navigation, and the position accuracy is lowered. In FIG. 37 (b), since the walking speed parameter and the yaw angle are corrected by the absolute position information, the position accuracy is improved.

  In the conventional technology, correction is performed to match the current position to the absolute position, but the positioning accuracy is continuously maintained because the yaw angle error and the walking speed estimation error are superimposed during inertial navigation and the position accuracy decreases. It was difficult to do. In the technology of the present embodiment, the yaw angle and the walking speed estimation parameter are updated at the same time when the absolute position information is observed, so that the positioning accuracy can be improved.

<Other application examples>
The best mode for carrying out the present invention has been described above with reference to the embodiments. However, the present invention is not limited to these embodiments, and various modifications can be made without departing from the scope of the present invention. And substitutions can be added.

  For example, in the present embodiment, the position estimation unit 800 estimates a position by inertial navigation using a Kalman filter as a parametric method, and the position is corrected with absolute position information. However, an estimation method such as a particle filter or an α-β filter may be used as a parametric method. In addition, filters in which these are modified or expanded may be used. Further, the position may be estimated by LMS (least-mean-square (LMS)) or the least steepest gradient method.

  4 and the like are divided according to main functions in order to facilitate understanding of processing by the inertial apparatus 1. The present invention is not limited by the way of dividing the processing unit or the name. The processing of the inertial device 1 can be divided into more processing units according to the processing content. Moreover, it can also divide | segment so that one process unit may contain many processes.

  Further, in the present embodiment, the inertial device 1 performs all the processing, but the inertial device 1 may detect acceleration, angular velocity, and geomagnetism and transmit them to the server. In this case, since the server estimates the position of the inertial device 1 and transmits it to the inertial device 1, the processing load on the inertial device 1 can be reduced.

  The position coordinate (P′1) in FIG. 1 is an example of a predetermined position, and the first observation update processing unit 502 or the second observation update processing unit 503 is an example of a position correction unit, and correction parameter estimation processing The unit 700 is an example of a correction parameter calculation unit, the walking speed correction parameter is an example of a movement speed correction parameter, and the coordinate system conversion processing unit 106 is an example of a traveling direction correction unit. The positioning history storage unit 703 is an example of a storage device, and the movement state detection unit 702 is an example of a storage unit. The distance A is an example of a first distance, and the distance B is an example of a second distance. The speed correction unit 350 is an example of a movement speed correction unit, and the positioning history storage unit 703 is an example of a position recording unit. The absolute position information acquisition unit is an example of the absolute position information input unit 400. Inertial device 1 is an example of the device itself, position 1001 is an example of an estimated position, and P′2 is an example of a correction position.

DESCRIPTION OF SYMBOLS 100 Attitude information conversion process part 200 Progression direction estimation part 300 Progression speed estimation part 400 Absolute position information input part 500 Absolute position estimation part 700 Correction parameter estimation process part 800 Position estimation part

Japanese Patent No. 5059933 JP 2013-050307 A

Greg Welch and Gary Bishop, "An Introduction to the Kalman Filter", Department of Computer Science, University of North Carolina at Chapel Hill, July 24, 2006 Toru Katayama, "New Version Kalman Filter", Asakura Shoten, January 20, 2000 I. M. Rekleitis, "A particle filter tutorial for mobile robot localization", Technical Report TR-CIM-04-02, Center for Intelligent Machines, McGill University, 2004

Claims (11)

A traveling direction estimation unit that estimates a traveling direction and a moving speed using the sensor output;
A position estimation unit that estimates the estimated position of the device by applying the traveling direction and the moving speed estimated by the traveling direction estimation unit to inertial navigation;
An absolute position information acquisition unit for acquiring absolute position information provided from the outside;
A position correcting unit that corrects the estimated position with the absolute position information to generate a corrected position when the absolute position information is acquired;
A traveling direction correction unit that corrects a traveling direction based on a difference in direction with respect to a predetermined position from each of the estimated position and the correction position;
Having inertial device. A correction parameter calculation unit that calculates a direction correction parameter for correcting the traveling direction based on a difference between a first direction connecting the estimated position and the predetermined position and a second direction connecting the correction position and the predetermined position; And
The inertial device according to claim 1, wherein the traveling direction correction unit corrects the traveling direction using the direction correction parameter. The absolute position information acquisition unit acquires the absolute position information every time the inertial device enters an area where the absolute position information is provided wirelessly,
The correction parameter calculation unit
The correction position corrected by the position correction unit when the absolute position information was acquired in the previous area, and the position correction unit corrected when the absolute position information was acquired in the current area. A first distance from the correction position;
The correction position corrected by the position correction unit when the absolute position information was acquired in the previous area and the position correction unit corrected when the absolute position information was acquired in the current area. Calculating a second distance from the previous correction position;
Using the ratio of the second distance to the first distance to calculate a movement speed correction parameter for correcting the movement speed;
A moving speed correction unit that corrects the moving speed using the moving speed correction parameter;
The inertial device according to claim 2, wherein the position estimating unit estimates the estimated position based on the moving speed corrected by the moving speed correcting unit.   The correction parameter calculation unit calculates the ratio of the second distance to the first distance, and the movement speed correction unit corrects the movement speed by multiplying the movement speed by the ratio. The inertial device described. The correction parameter calculating unit calculates the ratio of the second distance to the first distance;
The inertia according to claim 3, wherein the movement speed correction unit acquires the correction value from a table in which a correction value is associated with the ratio, and corrects the movement speed by multiplying the movement speed by the correction value. apparatus. The position correction unit estimates error information of the estimated position when correcting the estimated position using a parametric method,
The inertial device according to claim 2, wherein the correction parameter calculation unit changes the magnitude of the direction correction parameter in accordance with the magnitude of the error information. The absolute position information acquisition unit acquires the absolute position information every time the device enters the area where the absolute position information is provided wirelessly,
A position recording unit in which the estimated position estimated by the position estimation unit is stored in time series;
From the correction position corrected by the position correction unit when the absolute position information was acquired in the previous area, the position correction unit corrects the absolute position information when acquired in the current area. The correction parameter calculation unit calculates the direction correction parameter when the amount of movement based on the estimated position recorded in the position recording unit up to a predetermined distance is calculated. The inertial device described in 1. The position correction unit estimates error information of the estimated position when correcting the estimated position using a parametric method,
The inertial device according to claim 2, wherein the correction parameter calculation unit calculates the direction correction parameter when the error information is equal to or less than a threshold value.   The travel direction correction unit sets the direction correction parameter to a direction cosine representing the travel direction of a direction cosine matrix, and corrects the travel direction by multiplying the sensor output by the direction cosine matrix. The inertial device according to any one of the above. A traveling direction estimation unit that estimates a traveling direction and a moving speed using the sensor output;
A position estimation unit that estimates the estimated position of the device by applying the traveling direction and the moving speed estimated by the traveling direction estimation unit to inertial navigation;
An absolute position information acquisition unit for acquiring absolute position information provided from the outside;
A position correcting unit that corrects the estimated position with the absolute position information to generate a corrected position when the absolute position information is acquired;
A program for functioning as a traveling direction correction unit that corrects a traveling direction based on a difference in direction from a predetermined position from each of the estimated position and the correction position. A traveling direction estimation unit estimating a traveling direction and a moving speed using the sensor output;
A position estimating unit that applies the traveling direction and the moving speed estimated by the traveling direction estimating unit to inertial navigation to estimate an estimated position of the device;
An absolute position information acquisition unit acquiring absolute position information provided from outside;
A position correction unit that generates the corrected position by correcting the estimated position with the absolute position information when the absolute position information is acquired;
A traveling direction correction unit correcting a traveling direction based on a difference in direction from the estimated position and the corrected position to a predetermined position;
A positioning method having

Images