JP2017181218A - Portable terminal, program and method estimating forward direction of users using angular velocity sensor - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a portable terminal, a program and a method that estimate a turnaround of a user during moving along a passage on a map using an angular velocity sensor in accordance with a time elapse.SOLUTION: A portable terminal includes: map data storage means that stores a correct direction beforehand; angular velocity pose matrix calculation means that calculates an angular velocity pose matrix qat a time t from a matrix qexpressing rotation of a terminal pose on the basis of an angular velocity vector, and a terminal pose matrix qat a time t-1; forward direction estimation means that estimates a forward direction from the angular velocity pose matrix q; direction error calculation means that calculates a direction error Eserving as a difference between a correct direction and the forward direction; and error pose matrix calculation means calculates an error pose matrix qfrom an error having the angular velocity pose matrix qweighted with the direction error E, and feeds back and inputs the error pose matrix qto the angular velocity pose matrix calculation means at the time t+1 as a final terminal pose matrix qat the time t.SELECTED DRAWING: Figure 2

Description

  The present invention relates to a technique for detecting a user's trend using an angular velocity sensor.

  Conventionally, there is a technique for estimating a current position using a motion sensor (acceleration sensor, angular velocity sensor) (see, for example, Patent Document 1). According to this technique, the current position is estimated using an error between the traveling speed based on the waveform generated from the motion sensor and the actual traveling speed. Specifically, pedestrian positioning using the Kalman filter is realized including not only the terminal posture but also gyro offset (drift).

  FIG. 1 is a functional configuration diagram of a portable terminal in the prior art.

  FIG. 1 shows a technique for correcting a current position estimated by an angular velocity sensor by mapping it to map information. According to this technology, the portable terminal stores the advancing path information as map data. Then, a corner is detected from the forward direction of the mobile terminal calculated based on the change amount and integrated value of the angular velocity sensor. Specifically, it is detected as a corner when the direction changes more than a certain time within a certain time. For the detected corner, the traveling direction corresponding to the map information is estimated by matching. For example, when there is a cross passage, the traveling direction at the corner is specified as one of the options of straight ahead, 90 degrees, and -90 degrees. The user's forward direction is estimated according to the estimation result, and a service corresponding to the direction is provided by the application.

JP 2014-167461 A

"Multiple floor dead reckoning using sensors with built-in smartphones" Multimedia, Distributed Cooperation and Mobile Symposium 2013 Proceedings 2013, 723-735, 2013-07-03 “Kalman Filter”, [online], [Search on March 11, 2016], Internet <URL: https://en.wikipedia.org/wiki/%E3%82%AB%E3%83%AB%E3% 83% 9E% E3% 83% B3% E3% 83% 95% E3% 82% A3% E3% 83% AB% E3% 82% BF% E3% 83% BC> "Beginner, Kalman filter for beginners by beginners", [online], [Search March 11, 2016], Internet <URL: http://www1.accsnet.ne.jp/~aml00731/kalman. pdf> "Quaternion convenience note", [online], [March 11, 2016 search], Internet <URL: http: //www.mss.co.jp/technology/report/pdf/18-07.pdf>

However, according to the above-described prior art, it is based on the premise that “a person moves along a path on a map and changes direction only at a corner”. For this reason, there is a problem that when the user who holds the mobile terminal makes an unexpected movement such as avoiding a person in the cross passage, for example, the forward direction is erroneously determined. According to the position estimation using only the angular velocity sensor, if the corner is erroneously determined even once, then the position is continuously measured at a position significantly deviated from the original position.
Even if the traveling direction is not erroneously determined in the cross passage, the forward direction always outputs only the direction based on the map information as a candidate. Therefore, it is difficult to detect even an operation such as avoiding a person while traveling straight ahead.

  On the other hand, the inventors of the present application may have a problem that the direction change of the user at the corner is assumed to be along a path on the map. I thought. That is, when a change of direction at a corner of a moving user is detected, should it be determined as a gradual change of direction over time? I thought. In particular, is it possible to prevent a significant deterioration in the estimation performance in the forward direction even when the corner determination in the angular velocity sensor is wrong? I thought.

  Therefore, an object of the present invention is to provide a portable terminal, a program, and a method for estimating the direction change of a user who is moving along a path on a map according to the passage of time using an angular velocity sensor.

According to the present invention, the portable terminal has an angular velocity sensor and estimates the user's forward direction,
Map data storage means for storing the correct answer direction in advance;
Angular velocity posture for calculating angular velocity posture matrix q _t ^gyro at time t from matrix q ^gyro representing rotation of the terminal posture based on the angular velocity vector measured by the angular velocity sensor and terminal posture matrix q _{t-1 at} time t−1 Matrix calculation means;
Forward direction estimating means for estimating the forward direction from the angular velocity posture matrix q _t ^gyro ;
A direction error calculating means for calculating a direction error E _t which is a difference between the correct direction and the forward direction,
Calculates an error posture matrix q _t ^err from errors weighted by direction error E _t to the angular velocity posture matrix q _t ^Gyro, as the final terminal attitude matrix q _t the error posture matrix q _t ^err at time t, the time t + And an error posture matrix calculation means for feeding back to the angular velocity posture matrix calculation means in 1.

According to another embodiment of the mobile terminal of the present invention,
It is also preferable to output the forward direction estimated by the forward direction estimation means to the application.

According to another embodiment of the mobile terminal of the present invention,
Additional forward direction estimating means for estimating the forward direction from the error posture matrix q _t output from the error posture matrix calculating means,
It is also preferable to output the forward direction estimated by the additional forward direction estimation means to the application.

According to another embodiment of the mobile terminal of the present invention,
The error posture matrix calculation means is
A unit correction angle rE _t of a predetermined unit correction ratio r with respect to the direction error E _t is calculated,
It is also preferable to calculate the error posture matrix q _t ^err by adding the posture matrix representing the rotation about the vertical axis by the unit correction angle rE _t to the angular velocity posture matrix q _t ^gyro .

According to another embodiment of the mobile terminal of the present invention,
The angular velocity posture matrix calculation means preferably calculates the posture matrix using a Kalman filter.

According to another embodiment of the mobile terminal of the present invention,
An acceleration sensor;
From the angular velocity posture matrix q _t ^gyro using the difference e _t ^acc between the gravity vector g ^acc measured by the acceleration sensor and the gravity vector h (q _t ^gyro ) estimated from the angular velocity posture matrix q _t ^gyro An acceleration posture matrix calculating means for calculating a matrix q _t ^acc ,
The error posture matrix calculating means preferably calculates the error posture matrix q _t ^err from the acceleration posture matrix q _t ^acc instead of the angular velocity posture matrix q _t ^gyro .

According to another embodiment of the mobile terminal of the present invention,
The error posture matrix calculation means is
A unit correction angle rE _t of a predetermined unit correction ratio r with respect to the direction error E _t is calculated,
It is also preferable to calculate an error posture matrix q _t ^err by adding a posture matrix representing rotation about the vertical axis by the unit correction angle rE _t to the acceleration posture matrix q _t ^acc .

According to another embodiment of the mobile terminal of the present invention,
The angular velocity posture matrix calculating unit and the acceleration posture matrix calculating unit preferably calculate the posture matrix using a Kalman filter.

According to another embodiment of the mobile terminal of the present invention,
The error posture matrix calculation means is
Calculate the residual e ^err _t between the ^correct posture matrix q _t ^correct and the acceleration posture matrix q _t ^acc
An optimal Kalman gain K _t ^err is calculated using the covariance matrix P _t ^acc calculated by the Kalman filter of the acceleration posture matrix calculating means,
From the acceleration posture matrix q _t ^acc, calculates an error posture matrix q _t ^err based on the product of the residual e ^err _t and the optimal Kalman gain K _t ^err,
From the covariance matrix P _t ^acc, to calculate the covariance matrix P _t ^err based on optimal Kalman gain K _t ^err,
Furthermore, it is also preferable to feed back P _t ^err to the angular velocity posture matrix calculating means at time t + 1 as the final terminal posture matrix covariance matrix P _t at time t.

According to another embodiment of the mobile terminal of the present invention,
The terminal attitude matrices q _t and q _t ^gyro , q _t ^acc , q _t ^err are preferably quaternions (quaternions) expressed by a scalar part and a vector part.

According to another embodiment of the mobile terminal of the present invention,
The correct answer direction of the map data storage means is also preferably one or more advancing directions according to the direction of the passage from the position of the mobile terminal.

According to another embodiment of the mobile terminal of the present invention,
An initial posture matrix that calculates an initial angular velocity posture matrix q ₀ and outputs the angular velocity posture matrix q ₀ to the angular velocity posture matrix calculation means by regarding the acceleration vector a measured by the acceleration sensor as the gravity direction in the terminal coordinate system. It is also preferable to further have a calculation means.

According to the present invention, there is provided a program for causing a computer mounted on a device having an angular velocity sensor and estimating a user's forward direction to function.
Map data storage means for storing the correct answer direction in advance;
Angular velocity posture for calculating angular velocity posture matrix q _t ^gyro at time t from matrix q ^gyro representing rotation of the terminal posture based on the angular velocity vector measured by the angular velocity sensor and terminal posture matrix q _{t-1 at} time t−1 Matrix calculation means;
Forward direction estimating means for estimating the forward direction from the angular velocity posture matrix q _t ^gyro ;
A direction error calculating means for calculating a direction error E _t which is a difference between the correct direction and the forward direction,
Calculates an error posture matrix q _t ^err from errors weighted by direction error E _t to the angular velocity posture matrix q _t ^Gyro, as the final terminal attitude matrix q _t the error posture matrix q _t ^err at time t, the time t + The computer is caused to function as an error posture matrix calculation unit that feeds back and inputs to the angular velocity posture matrix calculation unit in 1.

According to another embodiment of the program of the present invention,
The apparatus further comprises an acceleration sensor,
From the angular velocity posture matrix q _t ^gyro using the difference e _t ^acc between the gravity vector g ^acc measured by the acceleration sensor and the gravity vector h (q _t ^gyro ) estimated from the angular velocity posture matrix q _t ^gyro The computer further functions as an acceleration posture matrix calculation means for calculating the matrix q _t ^{acc. The} error posture matrix calculation means calculates the error posture matrix q _t ^err from the acceleration posture matrix q _t ^acc instead of the angular velocity posture matrix q _t ^gyro. It is also preferable to make the computer function like this.

According to the present invention, there is provided a method for estimating a forward direction of an apparatus having an angular velocity sensor,
The apparatus has a map data storage unit that stores the correct answer direction in advance,
The device
A first angular velocity posture matrix q _t ^gyro at time t is calculated from a matrix q ^gyro representing the rotation of the terminal posture based on the angular velocity vector measured by the angular velocity sensor and the terminal posture matrix q _t-1 at time t−1. And the steps
A second step of estimating the forward direction from the angular velocity posture matrix q _t ^gyro ;
A third step of calculating a direction error E _t which is a difference between the correct direction and the forward direction,
Calculates an error posture matrix q _t ^err from errors weighted by direction error E _t to the angular velocity posture matrix q _t ^Gyro, as the final terminal attitude matrix q _t the error posture matrix q _t ^err at time t, the time t + And a fourth step of feeding back to the first step in step 1 and executing the fourth step.

According to another embodiment of the forward direction estimation method of the present invention,
The apparatus further comprises an acceleration sensor,
The device
From the angular velocity posture matrix q _t ^gyro using the difference e _t ^acc between the gravity vector g ^acc measured by the acceleration sensor and the gravity vector h (q _t ^gyro ) estimated from the angular velocity posture matrix q _t ^gyro Further performing the step of calculating the matrix q _t ^acc ;
In the fourth step, it is also preferable to calculate the error posture matrix q _t ^err from the acceleration posture matrix q _t ^acc instead of the angular velocity posture matrix q _t ^gyro .

  According to the mobile terminal, the program, and the method of the present invention, it is possible to estimate the direction change of the user who is moving along the path on the map according to the passage of time using the angular velocity sensor. In addition, since the corner determination in the angular velocity sensor is gradually determined, it is possible to prevent a significant deterioration in the estimation performance in the forward direction.

It is a functional block diagram of the portable terminal in a prior art. It is a 1st functional block diagram of the portable terminal in this invention. It is explanatory drawing showing the calculation formula of each function part in FIG. It is explanatory drawing showing the terminal coordinate system with respect to a portable terminal, and a world coordinate system. It is explanatory drawing showing estimation of the front direction in this invention. It is a 2nd function block diagram of the portable terminal in this invention.

  Below, the form for implementing this invention is demonstrated in detail using drawing.

FIG. 2 is a first functional configuration diagram of the mobile terminal according to the present invention.
FIG. 3 is an explanatory diagram illustrating calculation formulas of the respective functional units in FIG.

  According to FIG. 2, the portable terminal 1 possessed by the user is a terminal such as a smartphone, and includes an angular velocity sensor 101 and an acceleration sensor 102 as hardware. The mobile terminal 1 estimates the user's forward direction as software, and includes an initial posture calculation unit 11, an angular velocity posture matrix calculation unit 12, an acceleration posture matrix calculation unit 13, and a forward direction estimation unit 14. A direction error calculation unit 15, a map data storage unit 150, an error posture matrix calculation unit 16, and an additional forward direction estimation unit 17. These functional components are realized by executing a program that causes a computer mounted on the mobile terminal to function. Further, the processing flow of these functional components can also be understood as a method for estimating the forward direction of the apparatus.

[Angular velocity sensor 101]
The angular velocity sensor 101 is a sensor that detects a rotational movement with respect to a reference axis, and outputs an angle or an angular velocity. An angular velocity sensor that can be mounted on a mobile terminal is generally composed of MEMS (micro electro mechanical systems), and can measure a force acting in a direction orthogonal to the motion axis and the rotation axis of an object.

The angular velocity vector measured from the angular velocity sensor 101 at time t is expressed as follows.
ω _t = (ωx ωy ωz)
With respect to the angular velocity ω, the terminal rotation amount Δθ _{t at the} time t with respect to the sampling rate f of the angular velocity sensor is expressed as follows.
Δθ _t = ω _t / f
This rotation, the terminal coordinate system, [Delta] [theta] _t / | as the axis of rotation of, | | [Delta] [theta] _t is obtained by rotating only the magnitude | [Delta] [theta] _t.

[Acceleration sensor 102]
The acceleration sensor 102 measures three-dimensional (three-axis) acceleration (change in speed per unit time) in order to detect the terminal posture.
The acceleration vector measured from the acceleration sensor 102 at time t is expressed as follows.
a _t = (x _t , y _t , z _t )

[Initial posture calculation unit 11]
The initial posture calculation unit 11 determines the initial posture of the mobile terminal. As the initial posture, the acceleration vector a measured by the acceleration sensor 102 is regarded as the gravity direction g in the terminal coordinate system. The terminal posture is determined with the gravity direction g facing downward in the vertical direction.

Specifically, the initial posture matrix (quaternion) q ₀ is calculated as follows.
g: Gravity vector (= (0,0, -1))
a: Acceleration vector obtained from the acceleration sensor

na = a / | a | (| x | represents the magnitude of the vector x)
h = g + na / | g + na |
q ₀ = (na · h, na × h) (• is an inner product, × is an outer product)
The terminal attitude matrix q _t (the same ^{applies to} q _t ^gyro , q _t ^acc , and q _t ^err described below) is a quaternion expressed by a scalar part and a vector part (for example, Non-Patent Document 4). reference).

Then, the measured initial angular velocity posture matrix q ₀ is output to the angular velocity posture matrix calculation unit 12. Even if the initial posture is incorrect, it is corrected to the correct posture over time. Therefore, the initial posture calculation unit 11 is not essential and may give a fixed initial posture.

[Angular Velocity Posture Matrix Calculation Unit 12]
The angular velocity posture matrix calculation unit 12 calculates the angular velocity posture at time t from the matrix q ^gyro representing the rotation of the terminal posture based on the angular velocity vector measured by the angular velocity sensor 101 and the terminal posture matrix q _{t-1 at} time t−1. The matrix q _t ^gyro is calculated.
q _t ^gyro = q ^gyro × q _t-1
×: Operator of quaternion product The terminal posture matrix q _{t−1 at} time t−1 is input from the error posture matrix calculating unit 16.
Then, the calculated angular velocity posture matrix q _t ^gyro is output to the acceleration posture matrix calculation unit 13.

[Acceleration posture matrix calculation unit 13]
Acceleration posture matrix calculating unit 13 uses the gravity vector g ^acc measured by the acceleration sensor 102, the difference e _t ^acc of the angular velocity posture matrix q _t ^Gyro deduced from the gravity vector h (q _t ^gyro), an angular velocity An acceleration posture matrix q _t ^acc is calculated from the posture matrix q _t ^gyro .
q _t ^acc = q _t ^gyro + K _t ^{acc et} _t ^acc
The calculated acceleration posture matrix q _t ^acc is output to the forward direction estimation unit 14 and the error posture matrix calculation unit 16. q _t ^acc is an acceleration posture matrix based on the terminal posture before correction.

[Forward direction estimation unit 14]
The forward direction estimation unit 14 estimates the forward direction from the acceleration posture matrix q _t ^acc . For example, if it is assumed that the user is grasping the mobile terminal and walking while looking at the display, simply projecting the y-axis direction of the terminal in the world coordinate system onto the horizontal plane is the forward direction. I reckon.
The forward direction estimated by the forward direction estimation unit 14 is output to the application and the direction error calculation unit 15.

  FIG. 4 is an explanatory diagram showing a terminal coordinate system and a world coordinate system for the mobile terminal.

FIG. 4A shows a terminal coordinate system. The terminal coordinate system is a coordinate system obtained from the sensor of the terminal, and the right direction of the mobile terminal is the x-axis direction, the upward direction is the y-axis direction, and the surface direction is the z-axis.
According to FIG. 4B, the world coordinate system is represented. The world coordinate system is a Euclidean coordinate system with respect to the ground surface, and the horizontal plane is the xy plane and the vertical direction is the z-axis.
Note that which direction is the x axis and the y axis with respect to the horizontal plane is indefinite and is determined by the initial posture of the terminal. In particular, when only the angular velocity sensor and the acceleration sensor are used, the absolute direction (azimuth angle) on the horizontal plane cannot be determined. However, when a geomagnetic sensor is used, the absolute direction can be determined.

  Here, the forward direction estimated by the forward direction estimation unit 14 is the y-axis direction of the terminal coordinate system projected onto the world coordinate system plane.

[Map data storage unit 150]
The map data storage unit 150 stores the correct answer direction in advance. The “correct answer direction” is one or more advancing directions according to the direction of the passage from the position of the mobile terminal. For example, map information that the mobile terminal can pass through. The correct direction candidates are discretized directions (0 degrees, 90 degrees, 180 degrees, and 270 degrees) according to the path that can be traveled.

[Direction error calculation unit 15]
The direction error calculation unit 15 receives the forward direction from the forward direction estimation unit 14 and the correct direction from the map data storage unit 150. Then, the direction error calculation unit 15 searches for a correct direction having the smallest angle difference with respect to the forward direction. And according to this invention, a traveling direction will be shifted to the direction which cancels the angle difference.
Direction error calculating unit 15 calculates the direction error E _t which is a difference between the correct direction and the forward direction. The calculated direction error _Et is output to the error posture matrix calculation unit 16.

According to FIG. 4 (b), it has a y-axis of the terminal coordinate system of a mobile terminal projected to the world coordinate system horizontal plane, error between the correct direction in the world coordinate system, a direction error E _t. For example, it is assumed that the estimated y-axis (traveling direction) of the terminal coordinate system is 30 degrees, and the correct direction candidate is in units of 90 degrees. In this case, since the nearest discretized direction 30 degrees of 0 °, 0 ° regarded as correct direction, + 30 ° is direction error E _t.
As another embodiment, it is also possible to calculate the direction error using the map data, with the direction of the path closest to the estimated current position as the correct direction.

[Error posture matrix calculation unit 16]
Error posture matrix calculating unit 16 receives the acceleration posture matrix q _t ^acc from the acceleration posture matrix calculating unit 13, inputs a direction error E _t from the direction error calculating unit 15.
Error posture matrix calculating unit 16 calculates the error posture matrix q _t ^err from errors weighted by direction error E _t the acceleration posture matrix q _t ^acc. Then, the error posture matrix q _t ^err is fed back and input to the angular velocity posture matrix calculation unit 12 at time t + 1 as the final terminal posture matrix q _t at time t.

[Additional forward direction estimation unit 17]
The additional forward direction estimation unit 17 estimates the forward direction from the error posture matrix q _t output from the error posture matrix calculation unit 16. The forward direction estimated by the additional forward direction estimation unit 17 is output to the application.
Unlike the forward direction output from the forward direction estimation unit 14, the additional forward direction estimation unit 17 estimates the forward direction based on the posture after the posture update (the error posture matrix q _t of the error posture matrix calculation unit 16). To do. Therefore, it is possible to output an accurate forward direction based on the posture corrected at the current timing.

<< Embodiment for Computing Error Posture Matrix >>
According to the present invention, the above-described angular velocity posture matrix calculation unit 12, acceleration posture matrix calculation unit 13, and error posture matrix calculation unit 16 can be realized in the following four embodiments.
<First Embodiment: First Error Attitude Matrix Using Kalman Filter>
<Second Embodiment: Second Error Posture Matrix Using Kalman Filter>
<Third Embodiment: Error posture matrix based on unit correction angle of acceleration posture matrix>
<Fourth Embodiment: Error posture matrix based on unit correction angle of angular velocity posture matrix>

<First Embodiment: First Error Attitude Matrix Using Kalman Filter>
A “Kalman filter” is a continuous-time linear dynamic system used to estimate an amount that changes with time from observation with discrete errors (for example, see Non-Patent Documents 2 and 3). This is similar to the Hidden Markov Model, and future state variables can be statistically output by noise following a Gaussian distribution.

[Angular Velocity Posture Matrix Calculation Unit 12]
If the rotation expressed in the absolute coordinate system by the angular velocity sensor 101 is Δθ _t ^G , the angular velocity posture matrix calculation unit 12 can be expressed as follows using the terminal posture q _t−1 at time t−1.
Δθ _t ^G = q _t-1 Δθ _t
q: Quaternion (quaternion) representing rotation as a matrix

The absolute coordinate system, [Delta] [theta] _t ^G / | as the axis of rotation ^{_{of, | | Δθ t G Δθ t}} G | those rotated by, expressed as follows.
θ _t = | Δθ _t ^G |
n _t = Δθ _t ^G / | Δθ _t ^G |

According to the quaternion, when rotating by θ around a three-dimensional rotation axis vector n (size is 1), the rotation amount q ^gyro by the angular velocity sensor 101 can be expressed as follows using a four-dimensional vector. it can.
q ^gyro = (cos (θ _t / 2) n _t sin (θ _t / 2))
The angular velocity posture matrix q _t ^gyro that is the terminal posture at time t calculated from the angular velocity sensor is ^{obtained by} rotating the terminal posture q _t-1 at time t−1 by q ^gyro .
q _t ^gyro = q ^gyro × q _t-1
X: Operator of quaternion product Then, the angular velocity posture matrix calculation unit 12 executes the “prediction step” of the Kalman filter.
_{^{P t gyro = FP t-1}} F T + G k Q k G k T
F _t = ∂ / ∂q (q ^gyro · q) | _qt-1
The Kalman filter is expressed as a linear state equation characterized by time-varying matrices Fk, Gk, Hk, Qk, Rk.

The angular velocity posture matrix calculation unit 12 outputs the angular velocity posture matrix q _t ^gyro and the Kalman filter covariance matrix P _t ^gyro to the acceleration posture matrix calculation unit 13.

[Acceleration posture matrix calculation unit 13]
The acceleration posture matrix calculation unit 13 executes the “update step” of the Kalman filter.
g ^acc : acceleration vector (= gravity vector)
h (q): the gravity vector is estimated from the terminal posture _{^{q e t acc = g acc -h}} (q t gyro)
H _t ^acc = ∂ / ∂q (h (q)) | _qt ^{gyro
_{S t acc = H t acc P}} t gyro H t accT + R t acc
K _t ^acc = P _t ^gyro H _t ^accT S ^acc-1
q _t ^acc = q _t ^gyro + K _t ^{acc et} _t ^acc
P _t ^acc = (I−K _t ^acc H _t ^acc ) P _t ^gyro
The acceleration posture matrix calculation unit 13 outputs the acceleration posture matrix q _t ^acc and the Kalman filter covariance matrix P _t ^acc to the forward direction estimation unit 14 and the error posture matrix calculation unit 16.

[Error posture matrix calculation unit 16]
The error attitude matrix calculation unit 16 updates the terminal attitude as follows.
Terminal attitude ^q _t coreect the direction error E _t is 0 is obtained by rotating only -E _t a terminal attitude q _t ^acc before correction by the error in the vertical axis. The rotation of −E _t around the vertical axis is expressed using the following quaternion.
q _t ^v = (cos (−E _t / 2) 0 0 sin (−E _t / 2))
By using this, the terminal posture ^q _t coreect the direction error E _t is 0 can be calculated as follows.
q _t ^coreect = q _t ^v × q _t ^acc
(Attitude where q _t ^acc is rotated in the opposite direction around the vertical direction with the same size as the error)
q _t ^coreect : Observed value (terminal orientation with zero direction error)
q _t ^acc : Estimated value of internal state

By updating according to the update step of the Kalman filter, the next state can be calculated.
The (observation) residual e ^err _t between the ^correct orientation matrix x _t ^correct and the acceleration attitude matrix q _t ^acc is calculated as follows.
e _t ^err = q _t ^coreect −q _t ^acc
In addition, the covariance of the observation residual can be calculated as follows.
_{^{S t err = P t acc +}} R t err
R _t ^err : Parameter given to the Kalman filter from the outside (may be a fixed value)
By changing the parameter value of R _t ^err , the correction amount in one step can be adjusted.
The optimum Kalman gain K _t ^err is calculated as follows using the covariance matrix P _t ^acc calculated by the Kalman filter of the acceleration posture matrix calculation unit 13 (M ⁻¹ represents an inverse matrix of the matrix M). .
_{^{K t err = P t acc S}} t err-1
From the acceleration posture matrix q _t ^acc , an error posture matrix q _t ^err (estimated value of updated posture) based on the product of the residual e ^err _t and the optimum Kalman gain K _t ^err is calculated as follows.
_{^{q t err = q t acc +}} K T err e t err
From the covariance matrix P _t ^acc , a covariance matrix P _t based on the optimum Kalman gain K _t ^err is calculated as follows.
P _t ^err = (I−K _t ^err ) P _t ^acc

Then, the error posture matrix calculation unit 16 uses the error posture matrix q _t ^err and covariance matrix P _t ^err at time t as the final posture matrix q _t and covariance P _t at time t, and the angular velocity at time t + 1. Feedback to the attitude matrix calculation unit 12 is performed.

方向誤差Ｅ_tが０となる絶対座標系における端末のＹ軸方向ｙ_t ^coreectは、誤差による補正前の端末のＹ軸方向ｙ_t ^accを鉛直軸回りに、−Ｅ_tだけ回転させたものである。
誤差による補正前の端末のＹ軸方向ｙ_t ^accは、誤差による補正前の端末姿勢ｑ_t ^accを用いて計算することができる。具体的には、端末座標系でのｙ軸方向を表すベクトル、即ち、(0,1,0)をｑ_t ^accを用いて回転させればよい。即ち、以下のように計算することができる（Ｍ^Tは、行列Ｍの転置行列を表す）。
ｙ_t ^acc＝Ｒ(ｑ_t ^acc)(0,1,0)^T
鉛直軸回りに−Ｅ_tの回転は、以下のクォータニオンを用いて表される。
ｑ_t ^v＝（cos(−Ｅ_t/2) ０ ０ sin(−Ｅ_t/2)）
これを用いて、方向誤差Ｅ_tが０となる端末のＹ軸方向ｙ_t ^coreectは、以下のように算出することができる。
ｙ_t ^coreect＝Ｒ(ｑ_t ^v)ｙ_t ^acc
（ｙ_t ^accを鉛直方向回りに誤差と同じ大きさで逆方向に回転させたベクトル）
ｙ_t ^coreect：観測値（方向誤差が０となる端末のＹ軸方向）
ｙ_t ^acc：内部状態から推定値した端末のＹ軸方向 <Second Embodiment: Second Error Posture Matrix Using Kalman Filter>
The error attitude matrix calculation unit 16 updates the terminal attitude as follows.
A function representing conversion from quaternion q = (w, x, y, z) to a matrix representing rotation equivalent to rotation by quaternion is defined in advance as follows.

Y axis direction ^y _t coreect terminal in the absolute coordinate system direction error E _t is 0, the Y-axis direction y _t ^acc terminal before the correction by the error in the vertical axis, which was rotated by -E _t is there.
The Y-axis direction y _t ^acc of the terminal before correction by error can be calculated using the terminal posture q _t ^acc before correction by error. Specifically, a vector representing the y-axis direction in the terminal coordinate system, that is, (0,1,0) may be rotated using q _t ^acc . That is, it can be calculated as follows (M ^T represents a transposed matrix of the matrix M).
y _t ^acc = R (q _t ^acc ) (0,1,0) ^T
The rotation of −E _t around the vertical axis is expressed using the following quaternion.
q _t ^v = (cos (−E _t / 2) 0 0 sin (−E _t / 2))
Using this, Y axis direction ^y _t coreect terminals direction error E _t is 0, can be calculated as follows.
y _t ^coreect = R (q _t ^v ) y _t ^acc
(Vector obtained by rotating y _t ^acc in the reverse direction around the vertical direction with the same magnitude as the error)
y _t ^coreect : Observation value (Y-axis direction of the terminal where the direction error is 0)
y _t ^acc : Y-axis direction of terminal estimated from internal state

By updating according to the Kalman filter update step, the next state can be calculated.
The (observed) residual e ^err _t between the correct orientation posture matrix y _t ^correct and the acceleration posture matrix y _t ^acc is calculated as follows.
e _t ^err = y _t ^coreect −R (q _t ^acc ) (0,1,0) ^T
Note that R (q _t ^acc ) (0,1,0) ^T = y _t ^acc .
Further, the covariance of the observation residual can be calculated as follows.
_{^{S t err = HP t acc H}} T + R t err
R _t ^err : Parameter given to the Kalman filter from the outside (may be a fixed value)
H: Jacobian matrix of R (q) (0,1,0) ^T.
That is, H = ∂ / ∂q (R (q) (0,1,0) ^T ) | _qt ^acc
By changing the parameter value of R _t ^err , the correction amount in one step can be adjusted.
The optimum Kalman gain K _t ^err is calculated as follows using the covariance matrix P _t ^acc calculated by the Kalman filter of the acceleration posture matrix calculation unit 13 (M ⁻¹ represents an inverse matrix of the matrix M). .
_{^{K t err = P t acc S}} t err-1
From the acceleration posture matrix q _t ^acc , an error posture matrix q _t based on the product of the residual e ^err _t and the optimum Kalman gain K _t ^err is calculated as follows.
_{^{q t err = q t acc +}} K T err e t err
From the covariance matrix P _t ^acc , a covariance matrix P _t based on the optimum Kalman gain K _t ^err is calculated as follows.
P _t ^err = (I−K _t ^err ) P _t ^acc

Then, the error posture matrix calculation unit 16 uses the angular velocity posture matrix q _t ^err and covariance matrix P _t ^err at time t as the final posture matrix q _t and covariance P _t at time t, and the angular velocity at time t + 1. Feedback to the attitude matrix calculation unit 12 is performed.

<Third Embodiment: Error posture matrix based on unit correction angle of acceleration posture matrix>
The error posture matrix calculation unit 16 calculates an error posture matrix q _t ^err based on a predetermined unit correction ratio r. Also, ^let q _t ^{err be} the final posture matrix q _t at time t.
(1) Calculate the unit correction angle rE _t of a predetermined unit correction ratio r with respect to the direction error E _t.
(2) An attitude matrix representing rotation about the vertical axis by the unit correction angle rE _t is added to the acceleration attitude matrix q _t ^acc to calculate an error attitude matrix q _t ^err .

The unit correction ratio r (0 <r ≦ 1) represents a ratio with respect to an error in the angle corrected at a time. The angle corrected in one step is rE _t .
The corrected terminal posture q _t ^err is obtained by rotating the terminal posture q _t ^acc before correction due to an error by −rE _t around the vertical axis. The rotation of −rE _t about the vertical axis is expressed using the following quaternion.
q _t ^v = (cos (−rE _t / 2) 0 0 sin (−rE _t / 2))
q _t ^err = q _t ^v × q _t ^acc

<Fourth Embodiment: Error posture matrix based on unit correction angle of angular velocity posture matrix>
FIG. 5 is a second functional configuration diagram of the mobile terminal according to the present invention.

  According to FIG. 5 (3rd Embodiment), compared with FIG. 2 (1st Embodiment, 2nd Embodiment), the acceleration sensor 102 is not required, but only the angular velocity sensor 101 is used and the front direction is demonstrated. Can be estimated. Therefore, the functional configuration diagram of FIG. 5 is a high-level conceptual diagram of the functional configuration diagram of FIG.

The error posture matrix calculation unit 16 calculates an error posture matrix q _t ^err based on a predetermined unit correction ratio r. Also, ^let q _t ^{err be} the final posture matrix q _t at time t.
(1) Calculate the unit correction angle rE _t of a predetermined unit correction ratio r with respect to the direction error E _t.
(2) the angular velocity posture matrix q _t ^Gyro, by integrating the attitude matrix representing the rotation of the unit correction angle rE _t only about a vertical axis, and calculates the error posture matrix q _t ^err.

According to FIG. 5, the forward direction estimation unit 14 estimates the forward direction from the angular velocity posture matrix q _t ^acc .

  FIG. 6 is an explanatory diagram showing estimation of the forward direction in the present invention.

  FIG. 6A shows the estimation of the forward direction in the prior art (see, for example, Non-Patent Document 1). For example, when the correct direction candidate based on the map data is the right direction, when the forward direction of the mobile terminal is the right direction, it is determined that the vehicle turns right at the corner. That is, when a corner is detected, a discrete direction determination is performed in which the direction of the correct direction candidate is matched.

  On the other hand, according to FIG. 6B, the estimation of the forward direction in the present invention is represented. For example, when the correct direction candidate based on the map data is the right direction, even if the forward direction of the mobile terminal is the right direction, the correct direction candidate is gradually approached. For this reason, it is not determined that the vehicle has turned right immediately. Therefore, it is possible to follow the direction change to a direction other than the correct direction candidate to some extent. In particular, as shown in FIG. 6, according to the present invention, it is possible to prevent positioning in the correct direction determined erroneously due to the influence of noise during walking such as avoiding an obstacle.

  As described above in detail, according to the mobile terminal, the program, and the method of the present invention, the direction change of the moving user along the path on the map is estimated with the passage of time using the angular velocity sensor. can do. In addition, since the corner determination in the angular velocity sensor is gradually determined, it is possible to prevent a significant deterioration in the estimation performance in the forward direction.

  According to the various embodiments of the present invention described above, those skilled in the art can easily make various changes, modifications and omissions within the scope of the technical idea and the viewpoint of the present invention. The above description is merely an example, and is not intended to be restrictive. The invention is limited only as defined in the following claims and the equivalents thereto.

DESCRIPTION OF SYMBOLS 1 Mobile terminal 101 Angular velocity sensor 102 Acceleration sensor 11 Initial attitude | position calculation part 12 Angular velocity attitude | position matrix calculation part 13 Acceleration attitude | position matrix calculation part 14 Forward direction estimation part 15 Direction error calculation part 150 Map data storage part 16 Error attitude matrix calculation part 17 Additional Forward direction estimation unit

Claims (16)

A portable terminal that has an angular velocity sensor and estimates a user's forward direction,
Map data storage means for storing the correct answer direction in advance;
Angular velocity for calculating the angular velocity posture matrix q _t ^gyro at time t from the matrix q ^gyro representing the rotation of the terminal posture based on the angular velocity vector measured by the angular velocity sensor and the terminal posture matrix q _{t-1 at} time t−1. Attitude matrix calculation means;
Forward direction estimation means for estimating a forward direction from the angular velocity posture matrix q _t ^gyro ;
A direction error calculating means for calculating a direction error E _t which is a difference between the forward direction and the correct direction,
Wherein calculating the error posture matrix q _t ^err from errors weighted by the angular velocity posture matrix q _t ^Gyro in the direction error E _t, as the final terminal attitude matrix q _t the error posture matrix q _t ^err at time t, the time An error posture matrix calculation unit that feeds back and inputs to the angular velocity posture matrix calculation unit at t + 1. The mobile terminal according to claim 1, wherein the forward direction estimated by the forward direction estimation means is output to an application. Additional forward direction estimating means for estimating a forward direction from the error posture matrix q _t output from the error posture matrix calculating means,
The mobile terminal according to claim 1, wherein the forward direction estimated by the additional forward direction estimation unit is output to an application. The error posture matrix calculation means includes
Calculating a unit correction angle rE _t of a predetermined unit correction ratio r with respect to the direction error E _t,
The error attitude matrix q _t ^err is calculated by adding the attitude matrix representing rotation about the vertical axis by the unit correction angle rE _t to the angular velocity attitude matrix q _t ^gyro. The mobile terminal according to any one of the above. 5. The mobile terminal according to claim 1, wherein the angular velocity posture matrix calculating unit calculates a posture matrix using a Kalman filter. An acceleration sensor;
From the angular velocity posture matrix q _t ^gyro using the difference e _t ^acc between the gravity vector g ^acc measured by the acceleration sensor and the gravity vector h (q _t ^gyro ) estimated from the angular velocity posture matrix q _t ^gyro Acceleration posture matrix calculating means for calculating posture matrix q _t ^acc ,
6. The error posture matrix calculating unit calculates an error posture matrix q _t ^err from the acceleration posture matrix q _t ^acc instead of the angular velocity posture matrix q _t ^gyro. The portable terminal as described in. The error posture matrix calculation means includes
Calculating a unit correction angle rE _t of a predetermined unit correction ratio r with respect to the direction error E _t,
The error posture matrix q _t ^err is calculated by adding the posture matrix representing rotation about the vertical axis by the unit correction angle rE _t to the acceleration posture matrix q _t ^acc. Mobile devices. The mobile terminal according to claim 6, wherein the angular velocity posture matrix calculating unit and the acceleration posture matrix calculating unit calculate a posture matrix using a Kalman filter. The error posture matrix calculation means includes
Calculating a residual e ^err _t between the posture matrix q _t ^{correct in the} correct direction and the acceleration posture matrix q _t ^acc ;
An optimal Kalman gain K _t ^err is calculated using the covariance matrix P _t ^acc calculated by the Kalman filter of the acceleration posture matrix calculating means,
Wherein the acceleration posture matrix q _t ^acc, calculates an error posture matrix q _t ^err based on the product of the residual e ^err _t and the optimal Kalman gain K _t ^err,
From the covariance matrix P _t ^acc, to calculate the covariance matrix P _t ^err based on optimal Kalman gain K _t ^err,
9. The mobile terminal according to claim 8, wherein P _t ^err is fed back to the angular velocity posture matrix calculating means at time t + 1 as a final covariance matrix P _t of the terminal posture matrix at time t. . ^{10. The} terminal attitude matrix q _t and q _t ^gyro , q _t ^acc , q _t ^err are quaternions expressed by a scalar part and a vector part. The mobile terminal according to item 1. The said correct direction of the said map data storage means is one or more advancing directions according to the direction of a passage from the position of the said portable terminal, The one of Claim 1 to 10 characterized by the above-mentioned. Mobile device. By assuming the acceleration vector a measured by the acceleration sensor as the gravity direction in the terminal coordinate system, an initial angular velocity posture matrix q ₀ is calculated, and the angular velocity posture matrix q ₀ is output to the angular velocity posture matrix calculating means. The mobile terminal according to claim 1, further comprising a matrix calculation unit. A program for causing a computer mounted on a device having an angular velocity sensor and estimating a user's forward direction to function.
Map data storage means for storing the correct answer direction in advance;
Angular velocity for calculating the angular velocity posture matrix q _t ^gyro at time t from the matrix q ^gyro representing the rotation of the terminal posture based on the angular velocity vector measured by the angular velocity sensor and the terminal posture matrix q _{t-1 at} time t−1. Attitude matrix calculation means;
Forward direction estimation means for estimating a forward direction from the angular velocity posture matrix q _t ^gyro ;
A direction error calculating means for calculating a direction error E _t which is a difference between the forward direction and the correct direction,
Wherein calculating the error posture matrix q _t ^err from errors weighted by the angular velocity posture matrix q _t ^Gyro in the direction error E _t, as the final terminal attitude matrix q _t the error posture matrix q _t ^err at time t, the time A program that causes a computer to function as an error posture matrix calculation unit that feeds back and inputs to the angular velocity posture matrix calculation unit at t + 1. The apparatus further includes an acceleration sensor,
From the angular velocity posture matrix q _t ^gyro using the difference e _t ^acc between the gravity vector g ^acc measured by the acceleration sensor and the gravity vector h (q _t ^gyro ) estimated from the angular velocity posture matrix q _t ^gyro The computer further functions as an acceleration posture matrix calculating means for calculating the posture matrix q _t ^{acc. The} error posture matrix calculating means replaces the angular velocity posture matrix q _t ^gyro with the error posture matrix q _t from the acceleration posture matrix q _t ^acc. The program according to claim 13, wherein the program causes the computer to calculate ^err . A method for estimating the forward direction of a device having an angular velocity sensor, comprising:
The apparatus has a map data storage unit that stores a correct answer direction in advance,
The device is
An angular velocity posture matrix q _t ^gyro at time t is calculated from a matrix q ^gyro representing rotation of the terminal posture based on the angular velocity vector measured by the angular velocity sensor and a terminal posture matrix q _{t-1 at} time t−1. 1 step,
A second step of estimating a forward direction from the angular velocity posture matrix q _t ^gyro ;
A third step of calculating a direction error E _t which is a difference between the forward direction and the correct direction,
Wherein calculating the error posture matrix q _t ^err from errors weighted by the angular velocity posture matrix q _t ^Gyro in the direction error E _t, as the final terminal attitude matrix q _t the error posture matrix q _t ^err at time t, the time A method for estimating a forward direction of an apparatus, comprising: performing a fourth step that is fed back and input to a first step at t + 1. The apparatus further includes an acceleration sensor,
The device is
From the angular velocity posture matrix q _t ^gyro using the difference e _t ^acc between the gravity vector g ^acc measured by the acceleration sensor and the gravity vector h (q _t ^gyro ) estimated from the angular velocity posture matrix q _t ^gyro Further executing the step of calculating the attitude matrix q _t ^acc ;
^16. The forward direction estimation method for an apparatus according to claim 15, wherein the fourth step calculates an error posture matrix q _t ^err from the acceleration posture matrix q _t ^acc instead of the angular velocity posture matrix q _t ^gyro. .

Images