JP4774522B2 - State estimation device, state estimation method, state estimation program, and computer-readable recording medium - Google Patents

Description

  The present invention relates to a state estimation device, a state estimation method, a state estimation program, and a computer-readable recording medium that estimate a position where a car, a ship, or the like exists.

  In the current society, such as ships, automobiles, and aircraft, various moving bodies are used. There are many situations in which it is necessary to measure the state of a moving object, such as car navigation and ship condition monitoring.

  The state measurement method of the moving object is roughly classified into a dead reckoning method and a celestial navigation method. The celestial navigation method is a method of directly measuring global information using an external sensor such as a GPS or a camera. The dead reckoning method is an internal sensor such as a rotary encoder, gyro sensor, or acceleration sensor. This is a technique for measuring the amount of change from the initial state by continuously measuring minute changes and performing integration.

  The method of celestial navigation has no accumulated error and can directly measure the global state, but has the disadvantages of large dispersion of measurement values and long sampling time.

  On the other hand, the dead reckoning method has a short sampling time and a small dispersion of measured values. However, the dead reckoning method requires initial information in order to obtain global information, and is extremely vulnerable to disturbances that cannot be measured by internal sensors such as side skids of wheels, and has the disadvantage of accumulating measurement errors. is there.

  In order to overcome such drawbacks, research on sensor fusion using a combination of the above-described two methods has been actively conducted. A typical method is a method using a Kalman filter. The method using the Kalman filter uses a maximum likelihood estimation method to estimate the state of a moving object using an error variance matrix that evaluates the reliability of measurement values obtained by a dead reckoning method and a celestial navigation method.

  However, in the dead reckoning method, since errors accumulate according to the movement of the moving object, it is difficult to obtain an error variance matrix. In addition, since the method using the Kalman filter estimates the state of the moving object using the maximum likelihood estimation method, if the error variance matrix difference between the dead reckoning method and the celestial navigation method is large, , Almost no effect. Furthermore, when the initial state is unknown, the dead reckoning method cannot measure global information, and thus the estimation method using the Kalman filter cannot be used.

  The present invention has been made in view of the above-described conventional problems, and does not require an error variance matrix and can be used even when the initial state is unknown, a state estimation method, a state estimation program, and a computer-readable It is an object to provide a simple recording medium.

  In order to solve the above-described problem, the state estimation apparatus of the present invention includes a relative position measuring unit that measures the position of the moving body in the local coordinate system in which the coordinate axis changes according to the movement of the moving body as a relative position, and the moving body. Absolute position measuring means for measuring the position of the moving body in the global coordinate system with the coordinate axis fixed regardless of the movement of the absolute position, and the angle formed by the moving body with respect to the coordinate axis of the local coordinate system Relative angle measuring means for measuring as an angle, and from the relative position and the absolute position, an angle formed by the moving body with respect to the coordinate axis of the global coordinate system is calculated as an absolute posture angle. The initial posture angle is estimated on the assumption that the convergence value of the sequence represented by the recurrence formula including the absolute posture angle is equal to the initial posture angle indicating the posture angle of the moving object in the global coordinate system in the initial state. Current angle for estimating an initial angle estimation means and a current posture angle indicating a posture angle of the moving body in the global coordinate system in the current state as a sum of the relative posture angle and the estimated initial posture angle The estimation means and the convergence value of the numerical sequence expressed by the recurrence formula including the estimated initial attitude angle, the relative position, and the absolute position indicate the position of the moving object in the global coordinate system in the initial state. An initial position estimating means for estimating an initial position, a current position indicating a position of the moving object in the current state in the global coordinate system, the relative position, and the estimated initial position It is characterized by comprising current position estimating means for estimating based on the attitude angle and the estimated initial position.

  According to the above configuration, the current posture angle of the moving object is obtained by the current angle measuring means, for example, using Equation (1) described later without using the error variance matrix, and the current position of the moving object is estimated as the current position. It can be determined by means. Therefore, according to the present invention, unlike the conventional dead reckoning method, the current position of the moving object and the relative position measurement means and the relative angle measurement means can be obtained regardless of the accumulation of measurement errors in the internal sensor. The current posture angle can be estimated. In addition, according to the present invention, unlike the method using the Kalman filter, even if the difference in error variance matrix increases between the dead reckoning method and the celestial navigation method, The current posture angle can be estimated.

  Further, in the present invention, the initial posture angle of the moving body is estimated by the initial angle estimating means, and the initial position of the moving body is estimated by the initial position estimating means.

That is, in the initial angle estimating means, the convergence value (θ _o ) of the numerical sequence expressed by a recurrence formula (for example, formula (5) described later) including the relative attitude angle and the absolute attitude angle is The initial posture angle is estimated on the assumption that the moving body is equal to the initial posture angle indicating the posture angle in the global coordinate system. In the initial position estimating means, a convergence value (q of a numerical sequence expressed by a recurrence formula (for example, formula (8) described later) including the estimated initial posture angle, the relative position, and the absolute position is used. _The initial position is estimated on the assumption that _o ) is equal to the initial position indicating the position of the moving body in the global coordinate system in the initial state.

  Therefore, according to the present invention, unlike the method using the conventional Kalman filter, the current position and the current posture angle of the moving object can be estimated even when the initial state is unknown.

  In addition, the state estimation apparatus of the present invention includes a relative position measuring unit that measures the position of the moving body in the local coordinate system in which the coordinate axis changes according to the movement of the moving body as a relative position, regardless of the movement of the moving body. Absolute position measuring means for measuring the position of a moving object in a global coordinate system with a fixed coordinate axis as an absolute position, and a relative angle for measuring an angle formed by the moving object with respect to the coordinate axis of the local coordinate system as a relative posture angle It is expressed by a recurrence formula including a measuring unit, an absolute angle measuring unit that measures an angle formed by the moving body with respect to the coordinate axis of the global coordinate system as an absolute posture angle, and the relative posture angle and the absolute posture angle. Assuming that the convergence value of the sequence is equal to the initial posture angle indicating the posture angle of the moving object in the global coordinate system in the initial state, the initial angle estimating means for estimating the initial posture angle, and the movement in the current state Current angle estimation means for estimating a current posture angle indicating a posture angle in the global coordinate system as a sum of the relative posture angle and the estimated initial posture angle, and the estimated initial posture angle, Estimate the initial position by assuming that the convergence value of the numerical sequence expressed by the recurrence formula including the relative position and the absolute position is equal to the initial position indicating the position of the moving body in the global coordinate system in the initial state. Initial position estimating means, and a current position indicating a position of the moving object in the global coordinate system in the current state as the relative position, the estimated initial attitude angle, and the estimated initial position. And current position estimating means for estimating based on the current position.

  According to the above configuration, since the absolute angle measuring means is provided, the absolute posture angle of the moving body can be obtained with high accuracy in addition to the above-described effects. Therefore, the estimation accuracy of the value estimated based on the absolute posture angle in the present invention, that is, the initial posture angle and the initial position is improved, and the estimation accuracy of the current position and the current posture angle is also increased.

  Further, the state estimation method of the present invention includes a first step of measuring the position of the moving body in the local coordinate system in which the coordinate axis changes according to the movement of the moving body as a relative position, and the coordinate axis regardless of the movement of the moving body. A second step of measuring the position of the moving body in the global coordinate system in which is fixed as an absolute position, and a third step of measuring an angle formed by the moving body with respect to the coordinate axis of the local coordinate system as a relative attitude angle; The angle formed by the moving body with respect to the coordinate axis of the global coordinate system is calculated as an absolute posture angle from the relative position and the absolute position, and further, a recurrence formula including the relative posture angle and the absolute posture angle. Assuming that the convergence value of the expressed sequence is equal to the initial posture angle indicating the posture angle of the moving body in the global coordinate system in the initial state, the fourth step of estimating the initial posture angle and the current state A fifth step of estimating a current posture angle indicating a posture angle of the moving body in the global coordinate system as a sum of the relative posture angle and the estimated initial posture angle; and the estimated initial posture Assuming that the convergence value of the numerical sequence expressed by the recurrence formula including the angle, the relative position, and the absolute position is equal to the initial position indicating the position of the moving body in the global coordinate system in the initial state, the initial position And a current position indicating the position of the moving body in the global coordinate system in the current state, the relative position, the estimated initial attitude angle, and the estimated initial position, And a seventh step of estimating based on the above.

  According to the state estimation method having the above configuration, the same effects as those of the state estimation device of the present invention are obtained by the first step to the seventh step. be able to.

  Further, the state estimation method of the present invention includes a first step of measuring the position of the moving body in the local coordinate system in which the coordinate axis changes according to the movement of the moving body as a relative position, and the coordinate axis regardless of the movement of the moving body. A second step of measuring the position of the moving body in the global coordinate system in which is fixed as an absolute position, and a third step of measuring an angle formed by the moving body with respect to the coordinate axis of the local coordinate system as a relative attitude angle; A fourth step of measuring an angle formed by the moving body with respect to the coordinate axis of the global coordinate system as an absolute posture angle, and a convergence value of a numerical sequence expressed by a recurrence formula including the relative posture angle and the absolute posture angle Is assumed to be equal to the initial posture angle indicating the posture angle in the global coordinate system of the moving body in the initial state, and the fifth step of estimating the initial posture angle, and in the global coordinate system of the mobile body in the current state A sixth step of estimating a current posture angle indicating a posture angle of the current position as a sum of the relative posture angle and the estimated initial posture angle; the estimated initial posture angle; the relative position; and the absolute position. A seventh step of estimating an initial position on the assumption that a convergence value of a numerical sequence expressed by a recurrence formula including is equal to an initial position indicating a position of the moving body in the initial state in the global coordinate system; An eighth step of estimating a current position indicating a position of the moving body in the global coordinate system in a state based on the relative position, the estimated initial attitude angle, and the estimated initial position; The structure provided may be sufficient.

  According to the state estimation method having the above configuration, the same operation as the state estimation device of the present invention is realized by the first step to the eighth step, so the same effect as the state estimation device of the present invention is obtained. be able to.

It is a block diagram which shows the structure of the state estimation apparatus which concerns on one Embodiment of this invention. It is a figure for demonstrating the variable expressing the state of a moving body. It is a figure for demonstrating a local coordinate system and a global coordinate system. It is a figure for demonstrating the timing which complements local information. The moving body moves in the global coordinates from ( _{xg, nk} , yg _{, nk} ) to ( _{xg, n} , yg _{, n} ), and in the local coordinate system (x1 _{, nk).} , Y _{l, nk} ) to (x _{l, n} , y _{l, n} ). It is a figure which shows the structure of the system used by the effect confirmation experiment of this invention. It is a perspective view of the two-wheeled vehicle type robot used with the system of FIG. It is a bottom view of the two-wheeled vehicle type robot used with the system of FIG. It is a figure which shows the relationship between the position of 2 points | pieces which the vision system in the system of FIG. 6 measured about the two-wheeled vehicle type robot, and an attitude angle. It is a graph which shows the estimated value regarding each of the value (x, y, (theta)) obtained by the effect confirmation experiment of this invention. It is a graph which shows the estimated value of an initial state regarding the value (x) obtained by the effect confirmation experiment of this invention. It is a graph which shows the estimated value of an initial state regarding the value (y) obtained by the effect confirmation experiment of this invention. It is a graph which shows the estimated value of an initial state regarding the value ((theta)) obtained by the effect confirmation experiment of this invention.

Explanation of symbols

1 State Estimation Device 2 Relative Position Measuring Unit (Relative Position Measuring Means)
3 Absolute position measuring unit (Absolute position measuring means)
4 Relative angle measuring unit (relative angle measuring means)
5 Absolute angle measuring unit (Absolute angle measuring means)
6 Initial angle estimation unit (initial angle estimation means)
7 Current angle estimation unit (current angle estimation means)
8 Initial position estimation unit (initial position estimation means)
9 Current position estimation unit (current position estimation means)

[1. Configuration of state estimation device]
A configuration of a state estimation device according to an embodiment of the present invention will be described with reference to FIG. As shown in FIG. 1, the state estimation device 1 includes a relative position measurement unit 2, an absolute position measurement unit 3, a relative angle measurement unit 4, an absolute angle measurement unit 5, an initial angle estimation unit 6, and a current angle. An estimation unit 7, an initial position estimation unit 8, and a current position estimation unit 9 are provided.

  The relative position measuring unit 2 and the relative angle measuring unit 4 are realized by so-called internal sensors. Here, the internal sensor is a sensor that measures the relative position and the relative posture angle (relative posture angle) of the moving body. That is, the relative position measuring unit 2 represents the function of the inner world sensor to measure the relative position, and the relative angle measuring unit 4 represents the function of the inner world sensor to measure the relative posture angle. .

  The internal sensor can be realized by, for example, a sensor using odometry, an acceleration sensor, or a gyro sensor. Generally, (1) a measurement error is small, (2) a sampling rate is high, and (3) an initial value is It has four characteristics that it cannot be used when it is unknown, and (4) measurement errors accumulate.

  The absolute position measuring unit 3 and the absolute angle measuring unit 5 are realized by so-called external sensors. Here, the external sensor measures the absolute position (absolute position) or the absolute posture angle (absolute posture angle) of the moving body.

  For example, a GPS sensor, which is an example of an external sensor, can be used as the absolute position measuring unit 3 because it can measure the absolute position of a moving body. Further, a geomagnetic sensor, which is an example of an external sensor, can be used as the absolute angle measuring unit 5 because it can measure the absolute posture angle of the moving body. Furthermore, the vision system, the landmark sensor, and the range finder can be used as the absolute position measuring unit 3 and the absolute angle measuring unit 5 because they can measure the absolute position and the absolute posture angle of the moving body.

  A parameter that can specify the position of the moving object regardless of the state of the moving object, such as latitude and longitude, corresponds to the “absolute position”. Further, a parameter that can specify the posture angle of the moving body regardless of the state of the moving body, such as an angle with respect to the ground axis, corresponds to the “absolute posture angle”.

  The initial angle estimation unit 6 calculates the absolute posture angle in the initial state of the moving body based on the relative posture angle of the moving body obtained from the relative angle measuring unit 4 and the absolute posture angle of the moving body obtained from the absolute angle measuring unit 5. To be estimated. The calculation process in the initial angle estimation unit 6 will be described later.

  The current angle estimation unit 7 is based on the relative posture angle of the moving body obtained from the relative angle measurement unit 4 and the absolute posture angle in the initial state of the moving body estimated by the initial angle estimation unit 6. The absolute posture angle at is estimated. The calculation processing in the current angle estimation unit 7 will be described later.

  The initial position estimating unit 8 estimates the absolute position of the moving body in the initial state based on the relative position of the moving body obtained from the relative position measuring unit 2 and the absolute position of the moving body obtained from the absolute position measuring unit 3. It is. The calculation processing in the initial position estimation unit 8 will be described later.

  Based on the absolute position of the moving body in the initial state estimated by the initial position estimating section 8 and the relative position of the moving body obtained from the relative position measuring section 2, the current position estimating section 9 The position is estimated. The calculation processing in the current position estimation unit 9 will be described later.

  With the above configuration, the state estimation device 1 estimates the absolute position and absolute posture angle of the moving body in the initial state, and uses the absolute position and absolute posture angle to determine the absolute position and absolute position of the moving body in the current state. The posture angle is estimated. Hereinafter, the details of the arithmetic processing of each block constituting the state estimation device 1 will be described.

[2. (Explanation and preconditions of variables expressing the state of the moving object)
In order to uniquely represent a moving object that moves on a certain coordinate system, three state variables (x, y, θ) representing a position and a posture angle are required. Here, as shown in FIG. 2, the position q = [x, y] ^T represents the center of the moving body, and the attitude angle θ represents the relative angle between the moving body and the coordinate axis. Now, in the global coordinate system sigma _G given in, indicating the current state of the moving object (q _g, θ _g), considering that estimated by the state estimating apparatus 1.

Further, the following is assumed as a precondition for estimating (q _g , θ _g ) by the state estimation device 1.

(1) q _o and θ _o indicating the coordinates of the moving object in the initial state are unknown.

(2) It is assumed that the local information q _l and θ _l are amounts of change from the initial state measured by a dead reckoning method, and no error other than disturbance occurs.

(3) It is assumed that the global information q _g and θ _g are the current position and the attitude angle measured by a celestial navigation method, and white noise with an average of 0 is added to the measured value.

Under such a precondition, since the initial state q _o and θ _o are unknown, global information cannot be measured by a dead reckoning method. In addition, since the celestial navigation method adds white noise, the dispersion of measured values is large, and it is not preferable to use only this method. Furthermore, since the initial state q _o and θ _o are unknown, the Kalman filter cannot be used.

  These problems are solved by estimating global information using an initial state observer realized by the state estimation device 1 of the present embodiment. Hereinafter, the initial state observer will be specifically described.

[3. (Estimation of (q _g , θ _g ) using initial state observer)
First, as shown in FIG. 3, to define a local coordinate sigma _L. State of the mobile body in the global coordinate system _{Σ G (q g, θ g} ) can be expressed by the following equation (1).

ここで、ｑ_ｇ（＝［ｘ_ｇ，ｙ_ｇ］^Ｔ）およびθ_ｇはそれぞれ大域座標系Σ_Ｇでの位置と姿勢角を表し、ｑ_ｌ（＝［ｘ_ｌ，ｙ_ｌ］^Ｔ）およびθ_ｌは、それぞれ局所座標系Σ_Ｌでの位置および姿勢角を表すものである。また、ｑ_ｏ（＝［ｘ_ｏ，ｙ_ｏ］^Ｔ）およびθ_ｏは、それぞれ大域座標系Σ_Ｇでの初期状態における移動体の位置と姿勢角を表すものである。

Here, q _g (= [x _g , y _g ] ^T ) and θ _g represent the position and posture angle in the global coordinate system Σ _G , respectively, q _l (= [x _l , y _l ] ^T ) and θ _l is to represent the position and the posture angle of the local coordinate system sigma _{L. Further, q o (= [x o} , y o] T) and theta _o are those respectively representing the position and attitude angle of the mobile body in the initial state of the global coordinate system sigma _G.

In this specification, the position of the moving body in the local coordinate system sigma _L, the attitude angle, referred to as local information, each of the position and orientation angle of the mobile object in the local coordinate system sigma _L, "relative position", " It corresponds to “relative attitude angle”. The position in the global coordinate system sigma _G, refers to the attitude angle and the global information, respectively of the position and orientation angle of the global coordinate system sigma _G corresponds to the "absolute position", "absolute posture angle". T (x) is an attitude transformation matrix shown as follows.

もし、デッドレコニング的な手法で計測した局所情報ｑ_ｌ，θ_ｌに誤差が生じなければ、移動体の初期状態ｑ_ｏ，θ_ｏを推定し、座標変換式（１）式を用いることで大域情報（ｑ_ｇ，θ_ｇ）を推定することができる。

If there is no error in the local information q _l , θ _l measured by the dead reckoning method, the initial state q _o , θ _o of the moving body is estimated, and the global transformation is performed by using the coordinate transformation equation (1). Information (q _g , θ _g ) can be estimated.

[4. Derivation of initial position observer)
In the following, an observer for estimating the initial position of the moving object is derived under the condition that the initial position (position in the initial state) q _o is unknown and the initial posture angle (posture angle in the initial state) θ _o is known. .

  First, the following lemma is given to derive an observer that estimates the initial position from local information and global information.

Think (Lemma 2.1) mobile body shown in FIG. 2, and the local coordinate system shown in FIG. 3 for sigma _L and the global coordinate system sigma _G. Assuming that the initial position q _o of the moving object is known, a discrete time observer represented by the following equation (2) is given.

ここで、

here,

は、ｑ_ｏの推定値であり、単に「ｑ_ｏｂ，ｎ」と記載する場合もある。また、Ｋ：＝ｄｉａｇ（ｋ_ｘ，ｋ_ｙ）とする。

Is an estimated value of q _o , and may be simply described as “q _{ob, n} ”. Also, K: = diag (k _x , k _y ).

If the observer shown in this equation (2) satisfies 0 <k _x <2 and 0 <k _y <2 _{, then n ob is} q _{ob, n} → q _o .

(Proof)
Subtracting q _o from both sides of equation (2) yields:

したがって、−１＜１−ｋ_ｘ＜１かつ−１＜１−ｋ_ｙ＜１を満たすならば、ｎ→∞のとき、ｑ_ｏｂ，ｎ→ｑ_ｏとなる。

Therefore, if -1 <1-k _x <1 and -1 <1-k _y <1, then q _{ob, n} → q _o when n → ∞.

  Thus, Lemma 2.1 can be proved. Furthermore, if Lemma 2.1 is used, the following Lemma 2.2 regarding the initial position observer can be obtained.

(Lemma 2.2) mobile body shown in FIG. 2, and consider the local coordinate system sigma _L and the global coordinate system sigma _G shown in FIG. Assuming that the initial position q _o of the moving body is unknown and the initial posture angle θ _o is known, a discrete time observer represented by the following equation (3) is given.

このオブザーバは、０＜ｋ_ｘ＜２かつ０＜ｋ_ｙ＜２を満たすならば、ｎ→∞のとき、ｑ_ｏｂ，ｎ→ｑ_ｏとなる。

If this observer satisfies 0 <k _x <2 and 0 <k _y <2, then when n → ∞, q _{ob, n} → q _o .

  (Proof) From the equation (1), the following equation (4) is obtained.

また、式（３）および式（４）より、以下の方程式を導くことができる。

Further, the following equations can be derived from the equations (3) and (4).

よって、補題２．１より、−１＜１−ｋ_ｘ＜１かつ−１＜１−ｋ_ｙ＜１を満たすなら，ｎ→∞のとき、ｑ_ｏｂ，ｎ→ｑ_ｏとなる。

Thus, from Lemma 2.1, if -1 <1-k _x <1 and -1 <1-k _y <1, then q _{ob, n} → q _o when n → ∞.

[5. (Extension when initial posture angle is unknown)
Consider the case where the initial position q _o and the initial posture angle θ _o are unknown. First, we give the following proposition about the initial attitude angle observer.

Think (Proposition 2.1) mobile body shown in FIG. 2, and the local coordinate system shown in FIG. 3 for sigma _L and the global coordinate system sigma _G. Assuming that the initial posture angle θ _o of the moving object is unknown, a discrete time observer represented by the following equation (5) is given.

ここで、

here,

は、θ_ｏの推定値であり、単に「θ_ｏｂ，ｎ」と記載する場合もある。

Is an estimated value of θ _o , and may be simply described as “θ _{ob, n} ”.

_Further, θ _{l, n} is the attitude angle in the local coordinate system Σ _L, θ _{g, n} is the attitude angle in the global coordinate system sigma _G.

Further, in the equation _{(5), θ l, n} + θ ob, n -θ g, for values of _{n, -π <θ l, n} + θ ob, n -θ g, normalized to the range of _n ≦ [pi Suppose that

At this time, if 0 <k _θ <2 is satisfied, θ _{ob, n} → θ ₀ when n → ∞.

(Proof of Proposition 2.1)
Subtracting θ ₀ from both sides of equation (5) and substituting θ _{g, n} = θ _{l, n} + θ ₀ into equation (5) yields the following equation.

さらに、θ_ｏｂ，ｎ−θ_ｏ＝ｅ_θ，ｎを用いると、下式（６）を得ることができる。

Furthermore, when θ _{ob, n} −θ _o = e _{θ, n} is used, the following equation (6) can be obtained.

この式（６）に対して、

For this equation (6),

はＬｙａｐｕｎｏｖ関数となるから、ｎ→∞のときｅ_θ，ｎ→０となるので、ｎ→∞のときθ_ｏｂ，ｎ→θ_０となる（命題２．１の証明終わり）。

Since it becomes a Lyapunov function, since e _{θ, n} → 0 when n → ∞, θ _{ob, n} → θ ₀ when n → ∞ (end of proof of Proposition 2.1).

  Then, by using the proposition 2.1, the following theorem 2.1 can be obtained with respect to the initial state observer.

Think (Theorem 2.1) mobile body shown in FIG. 2, and the local coordinate system shown in FIG. 3 for sigma _L and the global coordinate system sigma _G. Assume that the initial position q _o of the moving body and the initial posture angle θ _o of the moving body are unknown, and a discrete-time observer represented by Expression (5) and the following Expression (8) is given.

ここで、

here,

は、ｑ_ｏの推定値であり、単に「ｑ_ｏｂ，ｎ」と記載する場合もある。また、ｑ_ｌ，ｎは局所座標系Σ_Ｌでの移動体の位置を示すものであり、ｑ_ｇ，ｎは、大域座標系Σ_Ｇでの移動体の位置を示すものであり、Ｋ：＝ｄｉａｇ（ｋ_ｘ，ｋ_ｙ）とする。

Is an estimated value of q _o , and may be simply described as “q _{ob, n} ”. Further, q _{l, n} is indicates the position of the movable body in the local coordinate system sigma _L, q _{g, n} is for indicating the position of the moving object in the global coordinate system Σ _G, K: = _Let diag (k _x , k _y ).

The observer shown in this equation (8) is such that q _{l, n} is bounded, and if 0 <k _x <2 and 0 <k _y <2, q _{ob, n} → q _o when n → ∞. It becomes.

(Proof)
Q _o is subtracted from both sides of Equation (8), and q _{g, n} = T (θ _o ) q _{l, n} + q _o is substituted into Equation (8). Further, when ε _n = q _{ob, n} −q _o , the following equation (9) is obtained.

さらに、式（１０）に示すｚを与える。

Furthermore, z shown in Formula (10) is given.

ｚの性質として、ｑ_ｌ，ｎが有界であるとの仮定および命題２．１より、ｎ→∞のときｚ→０となる。このｚを用いると、式（９）は下式（１１）に変形できる。

As the property of z, from the assumption that q _{l, n} is bounded and Proposition 2.1, z → 0 when n → ∞. Using this z, equation (9) can be transformed into the following equation (11).

と変形できる。ここで、ｚおよび状態ｑを入力とみなすと、式（１１）は離散時間ＩＳＳ（ｉｎｐｕｔ−ｔｏ−ｓｔａｔｅ ｓｔａｂｌｅ）となり、式（１１）は漸近安定となる。

And can be transformed. Here, when z and the state q are regarded as inputs, Expression (11) becomes a discrete time ISS (input-to-state table), and Expression (11) becomes asymptotically stable.

Therefore, if q _{l, n} is bounded and 0 <k _x <2 and 0 <k _y <2, then when n → 0, (q _{obs, n} , θ _{obs, n} ) → (q _o , Θ _o ).

[6. (Relation between equipment configuration and observer)
The state estimation device 1 (see FIG. 1) in the present embodiment estimates the initial state of the moving object using the above-described initial state observer equation (5) and equation (8), and further uses equation (1). Thus, the absolute position and the absolute posture angle in the current state of the moving body are estimated. The details will be described below.

  First, the process of the initial angle estimation unit 6 in the state estimation device 1 will be described. The initial angle estimation unit 6 calculates the absolute posture angle in the initial state of the moving body using the equation (5) indicating the initial state observer described above.

That is, the relative angle measurement unit 4 obtains θ _{l, n} in the equation (5) indicating the above-described initial state observer. Furthermore, θ _{g, n} in Expression (5) is acquired by the absolute angle measurement unit 5. Therefore, the initial angle estimation unit 6 substitutes θ _{l, n} and θ _{g, n} into equation (5), and uses the absolute attitude angle obtained from the absolute angle measurement unit 5 as an estimated value as θ _{ob, 0.} Thus, θ _o which is the convergence value of θ _{ob, n} can be estimated.

Next, processing in the current angle estimation unit 7 will be described. The current angle estimation unit 7 estimates the absolute posture angle in the current state of the moving body using the above-described equation (1). That is, the current angle estimator 7, as theta _l in the formula (1), by substituting theta _{l, n} obtained from the relative angle measuring unit 4, Furthermore, as theta _o in the formula (1), the initial angle estimation section 6 Substitute the one estimated by. Thereby, the current angle estimation unit 7 can calculate θ _g indicating the absolute posture angle in the current state of the moving body.

  Next, the process in the initial position estimation unit 8 will be described. The initial position estimation unit 8 calculates the absolute position in the initial state of the moving object using the above-described equation (8) indicating the initial state observer.

That is, the initial angle estimation unit 6 calculates θ _o which is the convergence value of θ _{ob, n} in Equation (8). Furthermore, q _{l, n} in equation (8) can be acquired from the relative position measurement unit 2, and q _{g, n} can be acquired from the absolute position measurement unit 3. Therefore, the initial position estimating unit 8 substitutes q _{l, n} and q _{g, n} into the equation (8), and uses the absolute position obtained from the absolute position measuring unit 3 as the estimated value as q _{ob, 0.} Thus, q _o can be estimated.

Finally, processing in the current position estimation unit 9 will be described. The current position estimation unit 9 estimates the absolute position of the moving object in the current state using Expression (1). That is, the current position estimation unit 9, the initial angle estimation unit 6 by substituting those estimated, q _l obtained from the relative position measuring section 2 as q _l, substituting _n as theta _o in the formula (1), substituting those estimated by the initial position estimation unit 8 as q _O. Thereby, the current position estimating unit 9 can calculate q _g indicating the absolute position of the moving object in the current state.

In this way, the state estimation device 1 estimates the initial state (θ _o , q _o ) of the moving body, and further _{calculates the} absolute position (q _g ) and the absolute attitude angle (θ _g ) in the current state of the moving body. ) Can be estimated. Furthermore, Expression (5) and Expression (8) function as a low-pass filter and can remove high-frequency noise.

[7. (Influence of measurement error)
Hereinafter, the influence of the error that occurs in the measurement value will be described. Now, it is assumed that a disturbance of Δq _{l at} the position and Δθ _{l at} the posture angle occurs due to skidding or the like at time k. At this time, the estimated value of local information after time k is expressed by the following equation (12).

ただし、ｎ≧ｋである。

However, n ≧ k.

  Note that the value on the left side in equation (12) indicates the estimated value of the local information,

および

and

の値が、局所情報の計測値を示している。

The value of indicates the measured value of the local information.

  The following lemma is given in order to consider the effect of the error in the local information on the estimated value of the initial state.

(Lemma 2.3) Consider the initial state observer shown in equations (5) and (8). Δq l to the position at time _k, and disturbance of Δθ _l has occurred in the attitude angle. At this time, when n → ∞, (q _{obs, n} , θ _{obs, n} ) → (q _o + T (θ _o ) q _{l, n−} T (θ _o + Δθ _l ) (q _{l, n} + Δq _l ), θ _o + Δθ _l ).

  (Proof) First, consider the initial attitude angle observer formula (5) after time k. From equation (12)

となる。命題２．１より、ｎ→∞のとき、θ_ｏｂ，ｎ→θ_{ｌ，ｎ＋Δθｌ}となる。

It becomes. From Proposition 2.1, when n → ∞, θ _{ob, n} → θ _{l, n + Δθl} .

Next, consider the initial position observer. Now, it is assumed that the estimated value θ _{ob, n} of the posture angle in the initial state has converged to θ _{l, n} + Δθ _l .

  At this time, the initial position observer is

となる。したがって、定理２．１より、ｎ→∞のとき、ｑ_ｏｂ，ｎ→ｑ_ｏ＋Ｔ（θ_ｏ）ｑ_ｌ，ｎ−Ｔ（θ_ｏ＋Δθ_ｌ）（ｑ_ｌ，ｎ＋Δｑ_ｌ）となる。

It becomes. Therefore, according to Theorem 2.1, when n → ∞, q _{ob, n} → q _o + T (θ _o ) q _{l, n−} T (θ _o + Δθ _l ) (q _{l, n} + Δq _l ).

  From Lemma 2.3, the following Theorem 2.2 holds for the estimated value of global information using the initial state observer.

(Theorem 2.2) Consider the moving body shown in FIG. 2, the local coordinate system Σ _L and the global coordinate system Σ _G shown in FIG. 3, and the initial state observer (equations (5) and (8)). Assume that a disturbance having a position Δql and a posture angle Δθl has occurred at time k. The following ^~ qg _{, n} , ^~ θg _{, n} are given.

ここで、^〜ｑ_ｇ，ｎ，および^〜θ_ｇ，ｎのそれぞれは、大域情報ｑ_ｇ，θ_ｇの推定値である。このとき、ｎ→∞ならば（^〜ｑ_ｇ，ｎ，^〜θ_ｇ，ｎ）→（ｑ_ｇ，ｎ，θ_ｇ，ｎ）となる。

^Here, each of the ~ _{q g, n,} and ^~ theta _{g, n,} is an estimate of the global information _{q g,} theta _g. In this case, n → ∞ if _{^{(~ q g, n, ~}} θ g, n) → (q g, n, θ g, n) to become.

(Proof) According to Lemma 2.3, when n → ∞, (q _{obs, n} , θ _o ) → (q _o + T (θ _o ) q _{l, n−} T (θ _o + Δθ _l ) (q _l , n + Δq _l ), θ _o + Δθ _l ). Substituting this into equation (13) for coordinate transformation,

となる。

It becomes.

  Theorem 2.2 shows that the use of the initial state observer can negate the effect of errors in local information. That is, the initial state observer functions as a low-pass filter and can remove high-frequency noise.

In other words, in the state estimation device 1 of the present embodiment, even if an error occurs in q _{l, n} acquired from the relative position measurement unit 2 or θ _{l, n} acquired from the relative angle measurement unit 4. By using the equations (5), (8), and (13), the current position (q _{g, n} , θ _{g, n} ) of the moving object can be estimated.

[8. Regarding the update timing of the initial state observer)
In order to calculate the absolute position and the absolute attitude angle of the moving body in the initial state using the update of the initial state observer expressed by the equations (5) and (8), that is, the equation (5) or the equation (8). The relative position, the relative attitude angle, the absolute position, and the absolute attitude angle need to be the same time.

  However, as shown in FIG. 4, the cycle of the measurement timing of the internal sensor is shorter than the measurement timing of the external sensor. Therefore, if the timing for estimating the initial state using the initial state observer is matched with the measurement timing of the external sensor, the relative position and the relative attitude angle at that timing may not be obtained.

  Therefore, it is preferable to complement the relative position and the relative attitude angle obtained from the internal sensor at the timing of estimating the initial state of the moving body using the initial state observer. Hereinafter, the procedure of the complement is demonstrated. The relative attitude angle is complemented by the relative angle measuring unit 4, and the relative position is complemented by the relative position measuring unit 2.

[8-1. (Prerequisite)
Consider the measurement conditions as follows.

  First, the absolute position / posture measurement values for n samples are described as follows.

なお、これらの絶対位置・姿勢の測定値は、単に「ｘ_ｇ，ｎ」「θ_ｇ，ｎ」と記載する場合もある。

Note that these absolute position / posture measurement values may be simply described as “x _{g, n} ” and “θ _{g, n} ”.

The time when the absolute position / orientation at the time of n samples is acquired is described as _{tg, n} .

  Furthermore, the absolute position / posture measurement values for m samples are described as follows.

なお、これらの絶対位置・姿勢の測定値は、単に「ｘ_ｌ，ｍ」「θ_ｌ，ｍ」と記載する場合もある。また、ｍサンプル時の絶対位置・姿勢を取得した時刻を、ｔ_ｌ，ｍとして記載する。

Note that the absolute position / posture measurement values may be simply described as “x _{l, m} ” and “θ _{l, m} ”. The time when the absolute position / orientation at the time of m samples is acquired is described as t _{l, m} .

  Here, consider the moment when the absolute position and orientation at the time of n samples are measured. Assume that the relative position and orientation are acquired up to m samples. At this time, the following two states can be described.

State 1. t _{g, n} ≦ t _{1, m}
State 2. t _{g, n} > t _{l, m}
State 1. In the case of (2), the absolute position at the time before the relative position / posture that has already been obtained is acquired. In this case, since new information is obtained from the time when the newest relative position / posture is acquired, the accuracy can be improved by separately treating these two states. In addition, state 1. in the case of,
t _{l, m−k−1} <t _{g, n} ≦ t _{l, m−k}
There exists an integer k ≧ 0 that satisfies

[8-2. Complementation when t _{g, n} ≦ t _{1, m} ]
In this case, the relative position / posture can be complemented easily. Although various methods are conceivable, in the present embodiment, a complementing method using (1) odometry, (2) extended odometry, and (3) linear interpolation will be described.

[8-2-1. Completion using odometry)
When using odometry, it is assumed that the attitude angular velocity ω _{l, m-k-1} and the translation velocity v _{l, m-k-1} at the time of m−k−1 samples are known.

At this time, the relative position (relative position) and orientation x _{l, n} , y _{l, n} , θ _{l, n at} t _{g, n} can be written as follows.

〔８−２−２．拡張オドメトリを用いた補完〕
拡張オドメトリを用いる場合、特別な情報は必要としない。この場合、（１）θ_{ｌ，ｍ−ｋ−１}＝θ_{ｌ，ｍ−ｋ}の場合、および（２）θ_{ｌ，ｍ−ｋ−１}≠θ_{ｌ，ｍ−ｋ}の場合に分けて補完する。

[8-2-2. Completion using extended odometry)
No special information is required when using extended odometry. In this case, the complement is divided into (1) θ _{l, m−k−1} = θ _{l, m−k} , and (2) θ _{l, m−k−1} ≠ θ _{l, m−k.} .

First, in the case of θ _{l, m−k−1} = θ _{l, m−k} , complementation is performed as follows.

また、θ_{ｌ，ｍ−ｋ−１}≠θ_{ｌ，ｍ−ｋ}の場合は、以下のようにまず角度を補完する。

When θ _{l, m−k−1} ≠ θ _{l, m−k} , the angle is first complemented as follows.

さらに、位置の補完のために、次のようなｒ_ａ，ｎ，ｒ_ｂ，ｎ，ｒ_ｎを考える。

Furthermore, because of the complementary position, consider following _{r a, n, r b,} n, the _{r n.}

このとき、ｘ_ｌ，ｎ，ｙ_ｌ，ｎは次のようになる。

At this time, x _{l, n} , y _{l, n} is as follows.

〔８−２−３．線形補完〕
ｘ_ｌ，ｎ，ｙ_ｌ，ｎ，θ_ｌ，ｎの線形補完は、下式に基づき行われる。

[8-2-3. (Linear completion)
Linear interpolation of x _{l, n} , y _{l, n} , θ _{l, n} is performed based on the following equation.

〔８−３．ｔ_ｇ，ｎ＞ｔ_ｌ，ｍの場合の補完〕
この場合、前節と若干異なり、（１）オドメトリを用いる補完、（２）拡張オドメトリを用いる補完、（３）速度・角速度が得られない場合の補完、および（４）線形補完について説明する。

[8-3. Complementation for t _{g, n} > t _{l, m} ]
In this case, slightly different from the previous section, (1) complement using odometry, (2) complement using extended odometry, (3) complement when speed / angular velocity cannot be obtained, and (4) linear complement will be described.

[8-3-1. Completion using odometry)
When odometry is used, it is assumed that the attitude angular velocity ω _{l, m} and the translation velocity v _{l, m} at m samples can be acquired.

At this time, the relative positions and orientations x _{l, n} , y _{l, n} and θ _{l, n at} t _{g, n} can be written as follows.

〔８−３−２．拡張オドメトリを用いた補完〕
拡張オドメトリを用いる場合も、ｍサンプル時の姿勢角速度ω_ｌ，ｍ，および並進速度ｖ_ｌ，ｍが取得可能であるとする。

[8-3-2. Completion using extended odometry)
Also in the case of using extended odometry, it is assumed that the attitude angular velocity ω _{l, m} and the translation velocity v _{l, m} at the time of m samples can be acquired.

  When using extended odometry, the angle is complemented as follows:

さらに、以下のような変数δ_ｌ，ｎを考える。

Further, consider the following variables δ _{l, n} .

δ _{l, n =} ω _{1, m} (t _{g, n} −t _{l, m)}
At this time, x _{l, n} and y _{l, n} are obtained as follows.

〔８−３−３．速度・角速度が得られない場合〕
この場合、（１）θ_{ｌ，ｍ−１}＝θ_ｌ，ｍの場合と、（２）θ_{ｌ，ｍ−１}≠θ_ｌ，ｍの場合とに分けて説明する。

[8-3-3. (If speed / angular velocity cannot be obtained)
In this case, the description will be divided into (1) θ _{l, m−1} = θ _{l, m} and (2) θ _{l, m−1} ≠ θ _{l, m} .

First, in the case of θ _{l, m−1} = θ _{l, m} , x _{l, n} , y _{l, n} , θ _{l, n} are complemented based on the following equation.

また、θ_{ｌ，ｍ−１}≠θ_ｌ，ｍの場合は、以下のように、まず角度を補完する。

When θ _{l, m−1} ≠ θ _{l, m} , the angle is first complemented as follows.

さらに、位置を補完するために、以下に示すｒ_ａ，ｎ，ｒ_ｂ，ｎ，ｒ_ｎを考える。

Furthermore, in order to complement the position, consider the following r _{a, n} , r _{b, n} , r _n .

このとき、ｘ_ｌ，ｎ，ｙ_ｌ，ｎは次のようになる。

At this time, x _{l, n} , y _{l, n} is as follows.

〔８−３−４．線形補完〕
線形補完は、下式に基づき行われる。

[8-3-4. (Linear completion)
Linear interpolation is performed based on the following equation.

〔９．絶対角度測定部がない場合〕
本実施形態の状態推定装置１においては、絶対角度測定部５は必須の構成ではない。すなわち、状態推定装置１において絶対角度測定部５を省略しても、後述するように、移動体の初期状態を推定することは可能である。

[9. (When there is no absolute angle measurement unit)
In the state estimation device 1 of the present embodiment, the absolute angle measurement unit 5 is not an essential configuration. That is, even if the absolute angle measurement unit 5 is omitted in the state estimation device 1, as described later, it is possible to estimate the initial state of the moving body.

First, as shown in FIG. 5, the moving object moves in the global coordinates from ( _{xg, nk} , yg _{, nk} ) to ( _{xg, n} , yg _{, n} ), and the local coordinate system. the inner _{(x l, n-k,} y l, n-k) from the moved _{(x l, n, y l} , n) to. In this case, the relative posture angle θ _{l, n} and the absolute posture angle θ _{g, n} of the moving body in the state after movement are expressed as the following equations.

_{θ l, n = arg ((} x l, n -x l, n-k) + i (y l, n -y l, n-k))
θ _{g, n} = arg ((x _{g, n} −x _{g, n−k} ) + i (y _{g, n} −y _{g, n−k} ))
Furthermore, by describing the initial angle observer expressed by the above equation (5) as the following equation, θ _o of the moving body in the initial state can be calculated.

θ _{ob, n + 1} = θ _{ob, n} −k _θ (θ _{g, n} −θ _{l, n} )
Here, in calculating θ _o , necessary parameters are (x _{g, n−k} , y _{g, n−k} ) and (x _{g, n} , y _{g, n} ). These parameters are obtained from the relative position measuring unit 2 and the absolute position measuring unit 3 (see FIG. 1). The parameters obtained from the relative angle measurement unit 4 and the absolute angle measurement unit 5 are not used to calculate _θo .

Therefore, in order to estimate the posture angle of the moving body in the initial state, the relative angle measuring unit 4 and the absolute angle measuring unit 5 can be omitted, and the above formula (x _{g, nk} , y _{g, n− It} can be said that the initial angle estimator 6 may perform an operation using _k ) and ( _{xg, n} , yg _{, n} ). However, if the absolute posture angle of the moving body in the current state is estimated, θ ₁ in equation (1) is required, and therefore the relative angle measurement unit 4 is required.

When the moving body does not move, that is, (x1 _{, n−} x1 _{, n−k} , _{y1, n− y1 , n−k} ) = (0,0) and ( _{xg, n− xg). , N−k} , y _{g, n−} y _{g, n−k} ) = (0, 0), θ _{l, n} and θ _{g, n} cannot be obtained as described above, but the current position Can be estimated. In addition, the estimation accuracy is improved by updating the initial position estimation unit and the relative position measurement unit as follows.

x _{ob, n + 1} = T (θ _{ob, n} ) x _l + x _{ob, n} x _l = 0
[10. (Verification experiment)
In order to confirm the effectiveness of the state estimation apparatus 1 described above, a control experiment using a two-wheeled vehicle type robot was performed. In this experiment, an improved odometry is used as a dead reckoning technique, and a position measurement system using a CCD camera is used as a celestial navigation technique.

[10-1. Experimental device〕
The system configuration used in this experiment is shown in FIG. As shown in FIG. 6, in the system used in this experiment, a two-wheeled vehicle type robot is used as a moving body, and the vision system (comprised of an image processor and a CCD camera) is controlled by a PC. Measured with

  The configuration of the two-wheeled vehicle type robot used in this experiment is shown in FIGS. 7 (a) and 7 (b). As shown in FIGS. 7 (a) and 7 (b), the two-wheeled vehicle type robot has a DC motor and a rotary encoder attached to both wheels, and the angles of the wheels can be measured independently. The two-wheeled vehicle type robot is controlled by a desktop PC, and the sampling time in this experiment is about 9 [msec].

  The sampling time of the vision system used in this experiment is about 17 [msec]. Further, since the posture angle of the robot cannot be directly measured in the vision system, the positions of two points are measured as shown in FIG. 8, and the posture angle is calculated using the following equation.

なお、ビジョンシステムの原点における計測値の誤差平均および誤差分散を、表１に示す。

Table 1 shows the error average and error variance of the measured values at the origin of the vision system.

〔１０−２．実験手法〕
初期状態（ｘ［ｃｍ］，ｙ［ｃｍ］，θ［ｒａｄ］）＝（５．００，−１０．００，０．００）から、目標状態（ｘ_ｒ［ｃｍ］，ｙ_ｒ［ｃｍ］，θ_ｒ［ｒａｄ］）＝（０．００，０．００，０．００）へと、二輪車両型ロボットの移動を制御した。さらに、その制御則として、同次有限時間整定制御則を用い、１８０秒間の制御を行った。

[10-2. Experimental method)
From the initial state (x [cm], y [cm], θ [rad]) = (5.00, −10.00, 0.00), the target state (x _r [cm], y _r [cm], The movement of the two-wheeled vehicle robot was controlled to θ _r [rad]) = (0.00, 0.00, 0.00). Further, as the control law, a homogeneous finite time settling control law was used, and the control was performed for 180 seconds.

A value obtained by measuring the initial state of the two-wheeled vehicle type robot with the vision system is used as the initial value of the initial state observer, and the observer gains (k _x , k _y , k _θ ) are (0.0007, 0.0007, 0). .0007). Furthermore, the estimated value estimated by the state estimation device of the present embodiment and the actually measured value measured using a caliper and a protractor are recorded for the point (final point) where the two-wheeled vehicle robot finally moved.

  In this experiment, the initial state observer is updated in synchronization with the sampling time of the vision system. Theoretically, updating the estimated value requires local information and global information at the same time, so if the sampling time of the astronomical navigation method and the dead reckoning method are different as in this case, the estimated value of the observer Influence. In this experiment, for the sake of simplification, it is assumed that the influence due to the difference in sampling time is sufficiently small, and no special interpolation is performed.

[10-3. Experimental result〕
The estimated value of global information is shown in FIG. 9, and the estimated value of the initial state is shown in FIGS. Table 2 shows estimated values and measured values of global information at the final point, and Table 3 shows error averages and error variances of the measured values and estimated values.

図１０より、ｘ_ｏｂ，ｎは制御開始から約８０秒後に５．１２［ｃｍ］に収束し、約１２０秒後から別の値に収束し始めていることが確認できる。また、図９を参照すると、約１２０秒後においてロボットが再び動いており、移動した際に横滑りなどの外乱が生じたと推測できる。同じことがｙ_ｏｂ，ｎおよびθ_ｏｂ，ｎについてもいえる。

From FIG. 10, it can be confirmed that x _{ob, n} converges to 5.12 [cm] after about 80 seconds from the start of control, and begins to converge to another value after about 120 seconds. In addition, referring to FIG. 9, it can be estimated that a disturbance such as skidding occurred when the robot moved again after about 120 seconds and moved. The same is true for _{yob, n} and _{θob, n} .

[11. Supplement)
Finally, each block of the state estimation device 1, particularly the initial angle estimation unit 6, the current angle estimation unit 7, the initial position estimation unit 8, and the current position estimation unit 9 may be configured by hardware logic, As described above, it may be realized by software using a CPU.

  That is, the state estimation device 1 includes a CPU (central processing unit) that executes instructions of a control program that implements each function, a ROM (read only memory) that stores the program, and a RAM (random access memory) that expands the program. And a storage device (recording medium) such as a memory for storing the program and various data. An object of the present invention is a recording medium on which a program code (execution format program, intermediate code program, source program) of a control program of the state estimation device 1 which is software for realizing the above-described functions is recorded so as to be readable by a computer. This can also be achieved by supplying the state estimation apparatus 1 and reading and executing the program code recorded on the recording medium by the computer (or CPU or MPU).

  Examples of the recording medium include a tape system such as a magnetic tape and a cassette tape, a disk system including a magnetic disk such as a flexible disk / hard disk and an optical disk such as a CD-ROM / MO / MD / DVD / CD-R, and an IC card. A card system such as an optical card (including a memory card) or a semiconductor memory system such as a mask ROM / EPROM / EEPROM / flash ROM can be used.

  Moreover, the state estimation apparatus 1 may be configured to be connectable to a communication network, and the program code may be supplied via the communication network. The communication network is not particularly limited. For example, the Internet, intranet, extranet, LAN, ISDN, VAN, CATV communication network, virtual private network, telephone line network, mobile communication network, satellite communication A net or the like is available. Further, the transmission medium constituting the communication network is not particularly limited. For example, even in the case of wired such as IEEE 1394, USB, power line carrier, cable TV line, telephone line, ADSL line, etc., infrared rays such as IrDA and remote control, Bluetooth ( (Registered trademark), 802.11 wireless, HDR, mobile phone network, satellite line, terrestrial digital network, and the like can also be used. The present invention can also be realized in the form of a computer data signal embedded in a carrier wave in which the program code is embodied by electronic transmission.

  The present invention is characterized in that the current position and the current posture angle of the moving object can be estimated even when the initial position and the initial posture angle of the moving object are unknown, without being affected by the relative posture angle and the error generated in the relative position. Have. In addition, unlike the method using the Kalman filter, an error variance matrix of measurement values is unnecessary, so that it can be used without any problem even when it is difficult to obtain error variance.

  Further, in the state estimation device of the present invention, the relative angle measuring means supplements the relative attitude angle already measured at the timing when the absolute angle measuring means measures the absolute angle, and the complemented relative attitude angle is It is preferable to output to the initial angle estimating means.

  In other words, at the timing when the initial angle estimation means estimates the initial posture angle, it is preferable that the values at the same time are aligned with respect to the relative posture angle and the absolute posture angle of the moving body. However, in general, the cycle in which the absolute angle measurement unit measures the absolute posture angle is longer than the cycle in which the relative angle measurement unit measures the relative posture angle. In some cases, corners cannot be obtained.

  Therefore, in the above configuration, the relative angle measurement unit outputs the complemented relative posture angle to the initial angle estimation unit. Therefore, even when the relative posture angle at the timing at which the initial posture angle is estimated is not measured by the relative angle measuring means, the complemented relative posture angle can be used instead of the measured value of the relative posture angle to The posture angle can be estimated.

  Further, in the state estimation device according to the present invention, the relative position measuring unit supplements the relative position already measured at the timing when the absolute position measuring unit measures the absolute position, and the complemented relative position is set as the initial position. It is preferable to output to the position estimation means.

  In other words, at the timing when the initial position estimating means estimates the initial position, it is preferable that the values at the same time are aligned with respect to the relative position and the absolute position of the moving body. However, in general, the cycle in which the absolute position measuring unit measures the absolute position is longer than the cycle in which the relative position measuring unit measures the relative position. Therefore, the relative position is obtained at the timing when the initial position is estimated. There may be no.

  Therefore, in the above configuration, the relative position measuring means outputs the complemented relative position to the initial position estimating means. Therefore, even if the relative position at the timing at which the initial position is estimated is not measured by the relative position measuring unit, the initial position is estimated by using the complemented relative position instead of the measured value of the relative position. be able to.

  It should be noted that the same effects as those of the state estimation apparatus of the present invention can be obtained using a computer by a state estimation program that causes a computer to execute some steps in the state estimation method configured as described above. Furthermore, by storing the state estimation program in a computer-readable recording medium, the state estimation program can be executed on any computer.

  According to the present invention, it is possible to estimate the current position and the current posture angle of a mobile object such as an automobile or a ship even when the initial position and the initial posture angle are unknown.

Claims (8)

Relative position measuring means for measuring the position of the moving body in the local coordinate system in which the coordinate axis changes according to the movement of the moving body as a relative position;
Absolute position measuring means for measuring the position of the moving body in the global coordinate system in which the coordinate axis is fixed irrespective of the movement of the moving body as an absolute position;
A relative angle measuring means for measuring an angle formed by the moving body with respect to the coordinate axis of the local coordinate system as a relative posture angle;
From the relative position and the absolute position, an angle formed by the moving body with respect to the coordinate axis of the global coordinate system is calculated as an absolute posture angle, and further expressed by a recurrence formula including the relative posture angle and the absolute posture angle. Initial angle estimation means for estimating the initial posture angle, assuming that the convergence value of the numerical sequence is equal to the initial posture angle indicating the posture angle in the global coordinate system of the moving object in the initial state,
Current angle estimation means for estimating a current posture angle indicating a posture angle of the moving body in the global coordinate system in a current state as a sum of the relative posture angle and the estimated initial posture angle;
The convergence value of the numerical sequence expressed by the recurrence formula including the estimated initial posture angle, the relative position, and the absolute position is an initial position indicating the position of the moving body in the global coordinate system in the initial state. An initial position estimating means for estimating an initial position as being equal;
Current position estimating means for estimating a current position indicating a position of the moving body in the global coordinate system in a current state based on the relative position, the estimated initial attitude angle, and the estimated initial position And a state estimation device. Relative position measuring means for measuring the position of the moving body in the local coordinate system in which the coordinate axis changes according to the movement of the moving body as a relative position;
Absolute position measuring means for measuring the position of the moving body in the global coordinate system in which the coordinate axis is fixed irrespective of the movement of the moving body as an absolute position;
A relative angle measuring means for measuring an angle formed by the moving body with respect to the coordinate axis of the local coordinate system as a relative posture angle;
Absolute angle measuring means for measuring an angle formed by the moving body with respect to the coordinate axis of the global coordinate system as an absolute posture angle;
Assuming that the convergence value of the numerical sequence expressed by the recurrence formula including the relative posture angle and the absolute posture angle is equal to the initial posture angle indicating the posture angle of the moving body in the global coordinate system in the initial state, An initial angle estimating means for estimating a posture angle;
Current angle estimation means for estimating a current posture angle indicating a posture angle of the moving body in the global coordinate system in a current state as a sum of the relative posture angle and the estimated initial posture angle;
The convergence value of the numerical sequence expressed by the recurrence formula including the estimated initial posture angle, the relative position, and the absolute position is an initial position indicating the position of the moving body in the global coordinate system in the initial state. An initial position estimating means for estimating an initial position as being equal;
Current position estimating means for estimating a current position indicating a position of the moving body in the global coordinate system in a current state based on the relative position, the estimated initial attitude angle, and the estimated initial position And a state estimation device.   The relative angle measuring means supplements the relative attitude angle already measured at the timing when the absolute angle measuring means measures the absolute angle, and outputs the complemented relative attitude angle to the initial angle estimating means. The state estimation apparatus according to claim 2, wherein   The relative position measuring means complements the relative position already measured at the timing when the absolute position measuring means measures the absolute position, and outputs the complemented relative position to the initial position estimating means. The state estimation apparatus according to claim 1 or 2. A first step of measuring, as a relative position, the position of the moving body in the local coordinate system in which the coordinate axis changes according to the movement of the moving body;
A second step of measuring, as an absolute position, the position of the moving body in the global coordinate system in which the coordinate axes are fixed regardless of the movement of the moving body;
A third step of measuring an angle formed by the moving body with respect to the coordinate axis of the local coordinate system as a relative posture angle;
From the relative position and the absolute position, an angle formed by the moving body with respect to the coordinate axis of the global coordinate system is calculated as an absolute posture angle, and further expressed by a recurrence formula including the relative posture angle and the absolute posture angle. A fourth step of estimating the initial posture angle, assuming that the convergence value of the sequence is equal to the initial posture angle indicating the posture angle in the global coordinate system of the moving body in the initial state;
A fifth step of estimating a current posture angle indicating a posture angle of the moving body in the global coordinate system in the current state as a sum of the relative posture angle and the estimated initial posture angle;
The convergence value of the numerical sequence expressed by the recurrence formula including the estimated initial posture angle, the relative position, and the absolute position is an initial position indicating the position of the moving body in the global coordinate system in the initial state. A sixth step for estimating the initial position as being equal;
A seventh step of estimating a current position indicating a position of the moving body in the current state in the global coordinate system based on the relative position, the estimated initial attitude angle, and the estimated initial position; A state estimation method comprising: A first step of measuring, as a relative position, the position of the moving body in the local coordinate system in which the coordinate axis changes according to the movement of the moving body;
A second step of measuring, as an absolute position, the position of the moving body in the global coordinate system in which the coordinate axes are fixed regardless of the movement of the moving body;
A third step of measuring an angle formed by the moving body with respect to the coordinate axis of the local coordinate system as a relative posture angle;
A fourth step of measuring an angle formed by the moving body with respect to the coordinate axis of the global coordinate system as an absolute posture angle;
Assuming that the convergence value of the numerical sequence expressed by the recurrence formula including the relative posture angle and the absolute posture angle is equal to the initial posture angle indicating the posture angle of the moving body in the global coordinate system in the initial state, A fifth step of estimating a posture angle;
A sixth step of estimating a current posture angle indicating a posture angle of the moving body in the global coordinate system in the current state as a sum of the relative posture angle and the estimated initial posture angle;
The convergence value of the numerical sequence expressed by the recurrence formula including the estimated initial posture angle, the relative position, and the absolute position is an initial position indicating the position of the moving body in the global coordinate system in the initial state. A seventh step to estimate the initial position as being equal;
An eighth step of estimating a current position indicating a position of the mobile body in the current state in the global coordinate system based on the relative position, the estimated initial attitude angle, and the estimated initial position; A state estimation method comprising:   A state that causes a computer to execute from the fourth step to the seventh step in the state estimation method according to claim 5, or from the fifth step to the eighth step in the state estimation method according to claim 6. Estimation program.   A computer-readable recording medium on which the state estimation program according to claim 7 is recorded.

Images