DE112006001314T5 - State estimation device, state estimation method, state estimation computer program, and computer readable storage medium - Google Patents

Abstract

State estimator with:
a relative position measuring device for measuring, as a relative position, the position of a vehicle in a location coordinate system whose coordinate axes change with a movement of the vehicle;
an absolute position measuring device for measuring, as an absolute position, the position of the vehicle in a global coordinate system whose coordinate axes are fixed independently of the movement of the vehicle;
relative angle measuring means for measuring, as a relative attitude angle, the angle of the vehicle with respect to one of the coordinate axes of the location coordinate system;
initial angle estimating means for calculating, as the absolute attitude angle, the angle of the vehicle with respect to one of the coordinate axes of the global coordinate system from the relative position and the absolute position, and also estimating an initial attitude angle which is the attitude angle of the vehicle in its initial state in the global coordinate system, assuming that a sequence represented by a recursive formula containing the relative attitude angle and the absolute attitude angle converges to a value corresponding to the initial attitude angle.

Description

TECHNICAL AREA

The Invention relates to state estimation devices, Processes, computer programs and computer-readable storage media for estimate the position of a motor vehicle, a ship or similar moving object.

BACKGROUND TECHNIQUE

In In our society today are different types of transport used by cars, ships and aircraft. In many situations, the condition of these types of vehicles must be below Use, for example, a vehicle navigation system or a ship condition monitoring be measured.

techniques for measuring the condition of a vehicle are roughly divided into two categories divided: dead reckoning and sky navigation. Celestial navigation techniques directly measure global information using external sensors, like the GPS and cameras. Dead reckoning techniques measure constantly using internal sensors, such as encoders, gyroscopes and accelerometers, tiny changes and integrate them, about the extent of the change across from to estimate the initial state. The sky navigation technique is free from an accumulation of Errors, and it can be a direct measurement of the global condition To run. The technique, however, produces large-scale metrics, and it requires one long sampling time.

on the other hand needed the dead reckoning technique a short sampling time and generates measured values with small distribution. However, the technique requires by nature from header information to obtain global information. she is also highly vulnerable for disturbances of Outside (for example, wheels, slipping sideways) that are not measured by internal sensors can be which can lead to a cumulative measurement error.

It run many research activities, around these shortcomings by combining the two techniques, or a sensor fusion, to take into account. A typical scheme involves the use of a Kalman filter. Through this scheme, the condition of a vehicle is determined by a procedure with maximum probability using an error distribution matrix estimated thereby reducing the reliability of Measured by a dead reckoning technique and a sky navigation technique.

however In a dead reckoning technique, it is difficult to have an error distribution matrix to formulate, as mistakes accumulate when a vehicle emotional. Furthermore In a Kalman filter technique, the condition of a vehicle becomes estimated by a maximum likelihood method. If the error distribution matrix between the dead reckoning technique and the sky navigation technique is very different, is a Kalman filter almost ineffective. Furthermore, a dead reckoning technique can not measure global information if the initial state is unknown; the state of a vehicle can not be determined using a Kalman filter estimated become.

DISCLOSURE OF THE INVENTION

Of the Invention which, in view of the conventional detailed above The task is based on a condition estimator, a state estimation method, a state estimation computer program as well as to create a computer-readable storage medium that does not rely on an error distribution matrix and still be of use are when the initial state is unknown.

The state estimating apparatus according to the invention, in order to take the problems into account, is characterized by comprising: a relative position measuring means for measuring, as a relative position, the position of a vehicle in a location coordinate system whose coordinate axes change with a movement of the vehicle ; an absolute position measuring device for measuring, as an absolute position, the position of the vehicle in a global coordinate system whose coordinate axes are fixed independently of the movement of the vehicle; a relative angle measuring device for measuring, as a relative position angle, the angle of the vehicle with respect to one of the coordinate axes of the Local coordinate system; initial angle estimating means for calculating, as the absolute attitude angle, the angle of the vehicle with respect to one of the coordinate axes of the global coordinate system from the relative position and the absolute position, and also estimating an initial attitude angle which is the attitude angle of the vehicle in its initial state in the global coordinate system, assuming that a sequence represented by a recursive formula containing the relative attitude angle and the absolute attitude angle converges to a value corresponding to the initial attitude angle; current angle estimation means for estimating, as a sum of the relative attitude angle and the estimated initial attitude angle, the current attitude angle which is the attitude angle of the vehicle in its current state in the global coordinate system; an initial position estimating means for estimating the initial position which is the position of the vehicle in its initial state in the global coordinate system on the assumption that a sequence represented by a recursive formula containing the estimated initial attitude angle, the relative position and the absolute position, converges to a value corresponding to the initial position; and current position estimation means for estimating the current position which is the position of the vehicle and its current state in the global coordinate system, based on the relative position, the estimated initial attitude angle, and the estimated initial position.

at the device configured in the above manner becomes the current one Position angle of the vehicle through the current-angle measuring device, for example using equations (1) (this will be explained later), without relying on an error distribution matrix. Deviating from usual Dead reckoning techniques, the invention may be the current location and the attitude angle of a vehicle regardless of a cumulative one Estimate measurement errors in the internal sensors, which serve as a relative position measuring device and realize the relative angle measuring device. Furthermore, the Invention, deviating from techniques based on a Kalman filter, the current position and attitude angle of the vehicle even then estimate if the error distribution matrices between the dead reckoning technique and of sky navigation technology differ greatly.

at appreciates the invention the initial angle estimator the initial one Position angle of the vehicle from. The initial position estimator estimates the starting position of the vehicle.

The initial angle estimator estimates the initial attitude angle, which is the attitude angle of the vehicle in its initial state in the global coordinate system, with the assumption that one by a recursive formula containing the relative attitude angle and the absolute attitude angle (e.g. Equation (5), which will be detailed later), sequence represented converges to a value (θ ₀ ) corresponding to the initial attitude angle. The initial position estimator estimates an initial position that is the position of the vehicle in its initial state in the global coordinate system, assuming that one by a recursive formula containing the estimated initial attitude angle, the relative position, and the absolute position (e.g. (8), which will be detailed later) converges to a value (q _o ) corresponding to the initial position.

Accordingly, the Invention, different from conventional Techniques based on a Kalman filter, the current position and estimate the attitude angle of the vehicle even if the initial state is unknown.

Alternatively, the state estimation apparatus according to the invention is characterized by being provided with: a relative position measuring means for measuring, as a relative position, the position of a vehicle in a location coordinate system whose coordinate axes change with a movement of the vehicle; an absolute position measuring device for measuring, as an absolute position, the position of the vehicle in a global coordinate system whose coordinate axes are fixed independently of the movement of the vehicle; relative angle measuring means for measuring, as a relative attitude angle, the angle of the vehicle with respect to one of the coordinate axes of the location coordinate system; absolute angle measuring means for measuring, as absolute attitude angle, the angle of the vehicle with respect to one of the coordinate axes of the global coordinate system; an initial angle estimating means for estimating an initial attitude angle which is the attitude angle of the vehicle in its initial state in the global coordinate system, assuming that a sequence represented by a recursive formula including the attitude angle and the absolute attitude angle converges to the initial attitude angle corresponding value; current angle estimation means for estimating, as a sum of the relative attitude angle and the estimated initial attitude angle, the current attitude angle which is the attitude angle of the vehicle in its current state in the Glo coordinate system; an initial position estimating means for estimating the initial position which is the position of the vehicle in its initial state in the global coordinate system on the assumption that a sequence represented by a recursive formula containing the estimated initial attitude angle, the relative position and the absolute position, converges to a value corresponding to the initial position; and current position estimation means for estimating the current position which is the position of the vehicle in its current state in the global coordinate system based on the relative position, the estimated initial attitude angle and the estimated initial position.

These Device is configured to use the absolute angle measuring device contains. Therefore, the device can achieve the above functions and effects and also get exactly the absolute attitude angle of a vehicle. Therefore, the invention may be the initial ones Position angle and the position with high accuracy from the absolute Position angle obtained. The invention can also be the current position and estimate the attitude angle with high accuracy.

The Zustandsabschätzverfahren according to the invention is characterized in that it includes the following steps: (1) Measure, as a relative position, the position of the vehicle in one Spatial coordinate system whose coordinate axes coincide with a movement change the vehicle; (2) measuring, as absolute position, the position of the vehicle in one Global coordinate system whose coordinate axes are independent of the movement of the vehicle are fixed; (3) Measure, as relative Attitude angle, the angle of the vehicle with respect to one of the coordinate axes the location coordinate system; (4) calculating, as the absolute positional angle, the angle of the vehicle with respect to one of the coordinate axes of the global coordinate system from the relative position and the absolute position, and also to estimate an initial one Attitude angle, the attitude angle of the vehicle in its initial state in the global coordinate system is assuming that a sequence, which is represented by a recursive formula that expresses the relative Position angle and the absolute attitude angle contains, on a the initial Position angle corresponding value converges; (5) Estimate, as Sum of the relative attitude angle and the estimated initial attitude angle, the current attitude angle, the attitude angle of the vehicle in the current state in the global coordinate system; (6) Estimate one Initial position, which is the position of the vehicle in its initial state in the global coordinate system, assuming that a sequence, which is represented by a recursive formula which is the estimated initial one Position angle, the relative position and the absolute position contains on converges a value corresponding to the initial position; and (7) Estimating the current position showing the position of the vehicle in its current position State in the global coordinate system is based on the relative position, of the estimated initial Attitude angle and the estimated Initial position.

The state estimation methods configured by the above steps (1) to (7) achieves the same operation as the state estimating apparatus according to the invention, and therefore achieves the same functions and effects as these.

alternative can the state estimation method according to the invention the following steps include: (1) measuring, as a relative position, the position of the vehicle in a location coordinate system whose Coordinate axes change with a movement of the vehicle; (2) Measuring, as absolute position, the position of the vehicle in one Global coordinate system whose coordinate axes are independent of the movement of the vehicle are fixed; (3) Measure, as relative Attitude angle, the angle of the vehicle with respect to one of the coordinate axes the location coordinate system; (4) measuring, as the absolute positional angle, the angle of the vehicle with respect to one of the coordinate axes the global coordinate system; (5) Estimate an initial one Attitude angle, the attitude angle of the vehicle in its initial state in the global coordinate system, assuming that a sequence, which is represented by a recursive formula that expresses the relative Position angle and the absolute attitude angle contains, on a the initial Position angle corresponding value converges; (6) Estimate, as Sum of the relative attitude angle and the estimated initial Position angle, the current attitude angle, the attitude angle of the vehicle is in its current state in the global coordinate system; (7) estimate an initial position showing the position of the vehicle in its Initial state in the global coordinate system is, assuming that a sequence represented by a recursive formula that is the estimated one initial Position angle, the relative position and the absolute position contains on converges a value corresponding to the initial position; and (8) estimate the current position, which is the position of the vehicle in its current state in the global coordinate system is on basis the relative position, the estimated initial Attitude angle and the estimated Initial position.

The state estimation methods configured by the above steps (1) to (8) achieves the same operation as the state estimator according to the invention, and therefore achieves the same functions and effects as these.

BRIEF DESCRIPTION OF THE DRAWINGS

1 : A block diagram illustrating the configuration of a state estimation device according to an embodiment of the invention.

2 : A diagram illustrating the state of a vehicle representing variable.

3 : A diagram that illustrates a location coordinate system and a global coordinate system.

4 : A diagram that illustrates times when local information is being replenished.

5 : A diagram showing a vehicle extending from ( _{xg, nk} , _{yg, nk} ) to ( _{xg, n} , _{yg, n} ) in the global coordinate system and ( _{xl, nk} , _{yl, nk} ) until (x _{l, n} , y _{l, n} ) moves in the location coordinate system.

6 : A diagram showing the configuration of a system used in an experiment in which the effects of the invention have been clarified.

7 (a) : An oblique view of the system in the 6 used two-wheeled, mobile robot.

7 (b) : A bottom view of the system in the 6 used two-wheeled, mobile robot.

8th : A diagram showing the relationship between the positions of two points and an absolute angle, as by a vision system in the system of 6 measured for the two-wheeled, mobile robot.

9 A graph showing estimates of x, y and θ obtained in an experiment in which the effects of the invention were clarified.

10 : A graph showing estimated initial values of x as obtained in an experiment in which the effects of the invention were clarified.

11 A graph showing estimated initial values of y as obtained in an experiment in which the effects of the invention were clarified.

12 : A graph showing estimated initial values of θ obtained in an experiment in which the effects of the invention were clarified.

BEST WAY TO EXECUTE INVENTION

1. Configuration of the state estimator

Now, the configuration of a state estimating device according to an embodiment of the invention will be described with reference to FIGS 1 described. As it is in the 1 has a state estimator 1 via an absolute angle measuring section 2 , an absolute position measuring section 3 , a relative angle measuring section 4 , an absolute angle measuring section 5 , an initial angle estimating section 6 , a current-angle estimating section 7 , an initial position estimating section 8th and a current position estimating section 9 ,

The absolute angle measuring section 2 and the relative angle measuring section 4 are provided using so-called internal sensors. In this case, internal sensors are those which measure the relative position and the attitude angle of the vehicle. In other words, the absolute angle measuring section represents 2 the relative position measuring function of the internal sensors, and the relative angle measuring section 4 represents the measurement function of the internal position angle sensors.

The internal sensors can be constructed, for example, from a distance measuring base sensor, an acceleration sensor or a gyro sensor. They generally have four properties (1) small measurement error; (2) high sampling rate; (3) not operable if initial values are unknown; and (4) cumulative measurement error.

The absolute position measuring section 3 and the absolute angle measuring section 5 are provided using so-called external sensors. In this case, external sensors are those which measure the absolute position or the attitude angle of the vehicle.

GPS sensors, which themselves constitute an example of an external sensor, can measure the absolute position of the vehicle, and therefore they can be used as an absolute position measuring section 3 be used. Earth magnetic field sensors, which form another example of external sensors, can measure the absolute attitude angle of the vehicle and therefore as an absolute angle measurement section 5 be used. Other examples include a vision system, a landmark sensor, and a range finder that can measure a vehicle's initial position and attitude angle, and therefore as an absolute position measurement section 3 and absolute angle measuring section 5 can be used. The absolute position is given by parameters that can identify the position of a vehicle regardless of its condition; an example is a combination of latitude and longitude. The absolute attitude angle is given by parameters that can identify the attitude angle of the vehicle regardless of its condition; an example is the angle of the vehicle with respect to the Earth's axis.

The initial angle estimation section 6 estimates the absolute attitude angle of the vehicle in the initial state from that of the relative angle measurement section 4 obtained relative attitude angle of the vehicle and the absolute angle measuring section 5 obtained absolute attitude angle of the vehicle. By the initial angle estimating section 6 Implemented computational processes will be detailed later.

The current-angle estimation section 7 estimates the absolute attitude angle of the vehicle in the current state from that of the relative angle measurement section 4 obtained relative attitude angle of the vehicle and by the initial angle estimation section 6 estimated absolute attitude angle of the vehicle in the initial state from. Through the current-angle estimation section 7 Implemented computational processes will be detailed later.

The initial position estimating section 8th estimates the absolute position of the vehicle in the initial state from the relative position of the vehicle obtained from the absolute angle measuring section and that from the absolute position measuring section 3 obtained absolute position of the vehicle. Through the initial position estimation section 8th Implemented computational processes will be detailed later.

The current position estimation section 9 estimates the absolute position of the vehicle in the current state from the initial position estimating section 8th estimated absolute position of the vehicle in the initial state and the relative position of the vehicle obtained from the absolute angle measuring section. Through the current position estimation section 9 Implemented computational processes will be detailed later.

The state estimator configured in the above manner estimates the absolute position and the attitude angle of a vehicle in the initial state. From this absolute position and the attitude angle estimates the device 1 also the absolute position and the attitude angle of the access attribute in the current state. Hereinafter, computational processes in each of the state estimation devices 1 forming blocks described.

2. Variable indicating the condition of a vehicle represent, and related assumptions

In order to uniquely represent a vehicle moving in a particular coordinate system, a set of three state variables x, y, and θ is needed that identify the position of the vehicle and the attitude angle. In the 2 q = [x, y] ^T denotes the position of the center of the vehicle, and θ indicates the attitude angle thereof, that is, the relative angle of the vehicle with respect to a coordinate axis. Now, the current state of the vehicle, q _g , θ _g , becomes a predetermined global coordinate system Σ _g using the state estimator 1 estimated.

• (1) Die Anfangswerte der Fahrzeugkoordinaten, q_o, θ_o, sind nicht bekannt.
• (2) Die Ortsinformation q_l, θ_l ist das Ausmaß der Änderung gegenüber dem Anfangszustand, wie durch Koppelnavigation gemessen. Es tritt mit Ausnahme des durch eine Störung von außen verursachten Fehlers kein Fehler auf.
• (3) Die Globalinformation q_g, θ_g ist die aktuelle Position und der Lagewinkel, wie durch Himmelsnavigation gemessen. Messwerte enthalten weißes Rauschen, das sich zu Nullrauschen herausmittelt.

The state estimator 1 creates an estimate for q _g and θ _g under the following assumptions:

• (1) The initial values of the vehicle coordinates, q _o , θ _o , are not known.
• (2) The location information q _l , θ _l is the amount of change from the initial state as described by Kop pelnavigation measured. There is no error except for the error caused by an external disturbance.
• (3) The global information q _g , θ _g is the current position and attitude angle as measured by sky navigation. Measured values contain white noise that comes out to zero noise.

Under these assumptions, dead reckoning does not work in measuring the global information because the initial values q _g , θ _{g are} unknown. However, the use of sky-only navigation is not desirable since its measurements contain white noise and inherently show a large distribution. Also, no Kalman filter can be used because the initial values q _o , θ _{o are} unknown.

The problems can be taken into account by estimating the global information using an initial state observation measure that is around the state estimator 1 constructed in the present embodiment. Hereinafter, the initial state observation measure will be described in concrete terms.

3. Estimate q _g , θ _g with the initial state observation measure

First, as it is in the 3 is represented, a spatial coordinate Σ _G defined. The state q _g , θ _{g of} the vehicle in the global coordinate system Σ _G is given by the set of equations (1): q G = T (θ 0 ) q l + q 0 θ G = θ l + θ 0 (1) where q _g (= [x _g , y _g ] ^T ) and θ _{g are} the position or the attitude angle in the global coordinate system Σ _G , q _l (= [x _l , y _l ] ^T ) and θ _l the position or the Position angles in the location coordinate system Σ _L are, and q _o (= [x _o , y _o ] ^T ) and θ _{o are} the initial position or the attitude angle in the global coordinate system Σ _G.

In this description, the position and the attitude angle of a vehicle in the location coordinate system Σ _{L are} collectively referred to as location information. The position and the attitude angle of the vehicle in the location coordinate system Σ _L correspond to the relative position and the relative attitude angle, respectively. In addition, the position and the attitude angle in the global coordinate system Σ _{G are} collectively referred to as global information. The position and the attitude angle in the global coordinate system Σ _G correspond to the absolute position and the absolute attitude angle, respectively. T (x) is a layer transformation matrix:

If the time measured by dead reckoning location information q _{l, l} θ contains no error can, the initial state of tape from tape q _o θ can be estimated _o the vehicle. Further, the global information q _g , θ _g can be estimated using the coordinate conversion equations (1).

4. Derivation of the initial state observation measure

Subsequently, an observation measure is derived which estimates the initial position of the vehicle under the conditions that this initial position q _o (position at the initial state) is unknown and that the initial attitude angle θ _o (attitude angle in the initial state) is known.

When The first is to derive an observational measure, which is the initial position from local and global information estimates, a lemma is given as follows.

Lemma 2.1

It will be in the 2 represented vehicle and the location coordinate system Σ _L and the global coordinate system Σ _G , as shown in the 3 are shown, assumed. The initial position q _{o of} the vehicle is known. The following equation (2) provides a discrete time observation measure: q ^ whether, n + 1 = q ^ whether n - K (q ^ whether n - q 0 ) (2) in which
q ^ _{ob, n}
is an estimate of q _o , which can be written simply as q _{ob, n} . In addition, K: = diag (k _x , k _y ).

In the observation measure formed by equation (2), q _{o , n} → q _o at n → ∞, if 0 <k _x <2 and 0 <k _y <2.

Proof of the Lemma 2.1

If q _{o is} subtracted from the two sides of equation (2), the following equation is obtained: q ^ whether, n + 1 - q 0 = q ^ whether n - q 0 - K (q ^ whether n - q 0 ) = (I - K) (q ^ whether n - q 0 )

Therefore q holds _{if, n} → q _o at n → ∞, if -1 <1 - k _x <1 and -1 <1 - k _y <1.

Now when the lemma 2.1 is proved in the above way, it becomes a lemma 2.2 to the initial state observation measure derived from the lemma 2.1.

Lemma 2.2

It was that in the 2 represented vehicle and the location coordinate system Σ _L and the global coordinate system Σ _G , as shown in the 3 are considered. The following equation (3), assuming that the initial position q _{o of} the vehicle is unknown and that the initial attitude angle θ _{o is} known, provides a discrete time observation measure: q ^ whether, n + 1 = q ^ whether n - K (T (θ 0 ) q l, n + q ^ whether n - q g, n ) (3)

In the observation measure, q _{ob, n} → q _o at n → ∞, if 0 <k _x <2 and 0 <k _y <2.

Proof of the Lemma 2.2

From equation (1) and equation (4), the following are obtained: q g, n = T (θ 0 ) q l, n + q 0 (4)

From the equations (3), (4), the following equation is derived: q ^ whether, n + 1 = q ^ whether n - K (q ^ whether n - q 0 )

Therefore, using the lemma 2.1 q _, we have _{ob, n} → q _o at n → ∞, if -1 <1 - k _x <1 and -1 <1 - k _y <1.

5. Application if the initial Position angle is unknown

It is assumed that neither the initial position q _o nor the initial attitude angle θ _{o are} known. First, consider an assertion regarding the initial state observation measure.

Claim 2.1

wobei
θ ^_ob,n
ein Schätzwert für θ_o ist, der einfach als θ_ob,n geschrieben werden kann.It was that in the 2 represented vehicle and the location coordinate system Σ _L and the global coordinate system Σ _G , as shown in the 3 are considered. The initial attitude angle θ _{o of} the vehicle is unknown. The following equation (5) provides a discrete time observation measure.

in which
θ ^ _{ob, n}
is an estimate for θ _o , which can simply be written as θ _{ob, n} .

In addition, θ _{1, n is} an attitude angle in the location coordinate system Σ _L , and θ _{g, n} is a attitude angle in the globalk coordinate system Σ _G.

In the equation (5), the value θ _{1, n} + θ _{ob, n} -θ _{g, n} is normalized such that -π <θ _{1, n} + θ _{ob, n} -θ _{g, n} ≦ π.

At 0 <k _θ <2, θ _{ob, n} → θ ₀ at n → ∞.

Prove the claim 2.1

Θ _{0 is} subtracted from both sides of the equation (5), and θ _{g, n} = θ _{1, n} + θ _{0 is} substituted into the equation (5), thereby obtaining: θ ^ whether, n + 1 - θ 0 = θ ^ whether n - θ 0 - k θ (θ l, n + θ ^ whether n - θ g, n ) = θ ^ whether n - θ 0 - k θ (θ ^ whether n - θ 0 )

Then, using θ _{ob, n} -θ _o = e _{θ, n} , an equation (6) is obtained. e θ n + 1 = e θ, n - k θ e θ, n (6)

In the equation (6) V = e 2 θ, n (7) a Lyapunov function. In this case, e _{θ, n} → 0 holds as n → ∞, which means that θ _{ob, n} → θ ₀ at n → ∞. So the assertion 2.1 is proven to be true.

Under Using Proposition 2.1, the following Theorem 2.1 is related to the initial state observation measure.

Theorem 2.1

It was that in the 2 represented vehicle and the location coordinate system Σ _L and the global coordinate system Σ _G , as shown in the 3 are considered. Neither the initial position q _{o of} the vehicle nor the initial attitude angle θ _{o of the} same is known. Equations (5) and (8) provide observational measures for the discrete time. q ^ whether, n + 1 = q ^ whether n - K (T (θ ^ whether n ) q l, n + q ^ whether n - q g, n ) (8th) in which
q ^ _{ob, n}
is an estimate for q _o , which can be written simply as q _{ob, n} . q _{l, n} is the position of the vehicle in the location coordinate system Σ _L , and q _{g, n} is the position of the vehicle in the global coordinate system Σ _G ; K: = diag (k _x, k _y).

In the observational measure given by equation (8), q holds _{ob, n} → q _o at n → ∞ when q _{1, n is} finite and 0 <k _x <2, and 0 <k _y <2.

Proof of the Theorem 2.1

If q _{o is} subtracted from the two sides of equation (8), q _{g, n} = T (θ _o ) q _{1, n} + q _{o is substituted} into equation (8), and ε _n = q _{ob, n} - q _{o is} introduced, equation (9) is obtained: ε n + 1 = (I - K) ε n - K [(T (θ ^ whether n ) - T (θ 0 )] Q l, n (9)

It is also introduced z as defined in equation (10): z = -K [(T (θ ^ whether n ) - T (θ 0 )] Q l, n (10)

Consider the assertion 2.1 and the assumption that q _{l, n is} finite. Also, n → ∞, z → 0. Using such z, the equation (9) reduces to an equation (11): ε n + 1 = (I - K) ε n + z (11)

If here it is assumed that to and the state q represent input values, Equation (11) provides a discrete time ISS (the input value for the State is stable), and it is asymptotically stable.

Therefore (q _{ob, n} , θ _{ob, n} ) → (q _o , θ _o ) at n → ∞, if q _{l, n is} finite and 0 <k _x <2 and 0 <k _y <2.

6. Relationship between the Device configuration and monitoring action

The state estimator 1 (see the 1 ) according to the present embodiment estimates the initial state of the vehicle using the equation (5), (8) to the initial state observing measure, and then estimates the absolute position and the absolute attitude angle of the vehicle in the current state using the equations (1) , The process is described in detail below.

First, the processing in the initial angle estimating section 6 in the state estimator 1 described. The initial angle estimation section 6 calculates the absolute attitude angle of the vehicle in the initial state using equation (5) to the initial state observation measure.

More specifically, the relative angle measuring section measures the value θ _{1, n} appearing in the equation (5) for the initial state observing measure. Meanwhile, the absolute angle measuring section measures 5 the value θ _{g, n} occurring in the equation (5). The initial angle estimation section 6 sets the values in θ _{i, n} and θ _{g, n} in the equation (5), and uses the one from the absolute angle measuring section 5 Estimate the absolute attitude angle as an estimate of θ _{ob, n} by θ _o , whereupon θ _{ob, n} converges.

Next, the processing in the current-angle estimation section becomes 7 described. The current-angle estimation section 7 estimates the absolute attitude angle of the vehicle in the current state using equations (1). More specifically, the current-angle estimating section sets 7 The value θ _{1, n} obtained from the relative angle measuring section enters into θ ₁ in the equations (1), and sets one by the initial angle estimating section 6 supplied estimate in θ _o in equations (1). Accordingly, the current-angle estimation section 7 calculate the value θ _g or the absolute attitude angle of the vehicle in the current state.

Next, the processing in the initial position estimating section 8th described. The initial position estimating section 8th calculates the absolute position of the vehicle in the initial state using equation (8) at the initial angle.

More specifically, the initial angle estimating section calculates 6 the value θ _o , to which θ _{ob, n} converges in the equation (8). Further, q _{l, n} in the equation (8) becomes the absolute angle measuring section 2 and q _{g, n} is obtained from the absolute position measuring section 3 receive. Therefore, the initial position estimating section sets 8th q _{l, n} and q _{g, n} into the equation (8), and it uses those from the absolute position measuring section 3 obtained absolute position as an estimate of q _{ob, 0} to estimate q _o .

Finally, the processing is in the current position estimating section 9 described. The current position estimation section 9 estimates the absolute position of the vehicle in the current state using equations (1). More specifically, the current position estimating section sets 9 one by the initial angle estimating section 6 supplied estimate in θ _o in the equations (1), he sets that of the absolute angle measuring section 2 obtained value q _{l, n} in q _l , and sets one by the initial position estimating section 8th delivered estimate in q _o . Accordingly, the current position estimating section can 9 Calculate q _g or the absolute position of the vehicle in the current state.

The state estimator 1 thus estimates the initial state θ _o , q _{o of} the vehicle, and estimates the absolute position q _g and the attitude angle θ _{g of} the vehicle in the present state. Furthermore, equations (5), (8) act as low-pass filters that remove high-frequency noise.

7. Undesirable effect of measurement errors

mit n ≥ k.Hereinafter, adverse effects of errors occurring in the measurement will be described. It be external disturbances Δq _l and Δθ _l assumed at the position or the attitude angle, for example, occur due to a sideways slide at time k. Under these conditions, a set of equations (12) provides estimates of location information at time points after time k.

with n ≥ k.

The values on the left side of equations (12) are the estimates of the location information. The values of
q - _{l, n}
and
θ - _{l, n}
on the right side are the measured values of the location information.

One Lemma is given as follows to avoid the effect of the error in the Location information measurement to the estimated initial values.

Lemma 2.3

The initial state observational measures given by equations (5), (8) will now be discussed. It is assumed that at the time k for the position and the attitude angle external disturbances Δq _l and Δθ _l apply. Under these conditions, for the initial state observing measure q, _{ob, n} , θ _{ob, n} ) → (q _o + T (θ _o ) q _{1, n} -T (θ _o + Δ θ ₁ ) (q _{1, n} + Δq ₁₎ ), θ _o + Δθ _l ) when n → ∞.

Proof of the Lemma 2.3

First, consider the equation (5) for the initial state observation measure at time points after the time k. From equations (12) the following is obtained:

If the assertion 2.1 is taken into account, θ _{ob, n} → θ _{1, n + Δθ 1} at n → ∞.

Next, the initial state observing measure will be discussed. It is assumed that the estimated value θ _{whether n} of the initial attitude angle θ against _{l, n} + _l Δθ converges.

Under these conditions, the initial initial state observation measure becomes: q ^ whether, n + 1 = q ^ whether n - K (T (θ ^ whether n ) q - l, n + q ^ whether n - q g, n ) = q ^ whether n - K [q ^ whether n - (q 0 + T (θ 0 ) q l, n - T (θ ^ whether n ) q - l, n )]

Therefore, if theorem 2.1 is taken into account, then q _{, n} → q _o + T (θ _o ) q _{1, n} -T (θ _o + Δθ ₁ ) (q _{1, n} + Δq ₁ ) at n → ∞.

Out In Lemma 2.3, the following Theorem 2.2 becomes in relation to the estimates derived the global information using the initial state observation measure.

Theorem 2.2

wobei q ~_g,n und θ ~_g,n die Schätzwerte der Globalinformation q_g bzw. θ_g sind. Unter diesen Bedingungen gilt (q ~_g,n,

_g,n) → (q_g,n, θ_g,n) bei n→∞.It was that in the 2 represented vehicle and the location coordinate system Σ _L and the global coordinate system Σ _G , as shown in the 3 and the initial state observation measures (equations (5), (8)) are considered. It is assumed that, at time k, external disturbances Δq ₁ and Δθ ₁ occurred at the position and the attitude angle. Then q - _{g, n} , θ - _{g, n} are as given below:

where q ~ _{g, n} and θ ~ _{g, n are} the estimates of the global information q _g and θ _g , respectively. Under these conditions, (q ~ _{g, n} ,

_{g, n} ) → (q _{g, n} , θ _{g, n} ) at n → ∞.

Proof of the Theorem 2.2

From the lemma 2.3 it follows (q _{ob, n} , θ _o ) → (q _o + T (θ _o ) q _{1, n} -T (θ _o + Δ θ ₁ ) (q ₁ , n + Δq ₁ ), θ _o + Δθ _l ) at n → ∞. When used in the coordinate conversion equation (13), the following is obtained: q ~ g, n = T (θ ^ whether n ) q - l, n + q ^ whether n = T (θ 0 + Δθ l ) (Q l, n + Δq l ) + q 0 + T (θ 0 ) q l, n - T (θ 0 + Δθ l ) (Q l, n + Δq l ) = q g, n θ ~ g, n = θ - l, n + θ ^ whether n = θ l, n - Δθ l + θ 0 + Δθ l = θ g, n

The Theorem 2.2 demonstrates that using the initial state observational measures the effect of an error in the location information is canceled can. In other words, the initial state observational measures act as low-pass filters that remove high-frequency noise.

In summary, the state estimator 1 According to the present embodiment, the actual position (q _{g, n} , θ _{g, n} ) of the vehicle is estimated using equations (5), (8) and (13) even when passing through the absolute angle measuring section 2 obtained value q _{l, n} under the by the relative angle measuring section 4 obtained value θ _{l, n} contains errors.

8. Timings for Updating Initial State Observation Measures

Around the initial state observational measures given by equations (5), (8) to update, that is, the absolute position and the absolute angle of the vehicle in the initial state using these equations are values at the same time for the relative position, the relative attitude angle, the absolute position and the absolute attitude angle required.

However, as it is in the lead 4 is shown, the internal sensors take measurements with a shorter cycle than the external sensors. Therefore, it is when the timing according to which the initial state observing measure estimates the initial state is made to coincide with a timing according to which the external sensors make a measurement, it is possible that the relative position and the attitude angle are not obtained at that time.

Accordingly, it is preferable to replicate the relative position and the attitude angle as obtained by the internal sensors when the initial state observing measure estimates the initial state of the vehicle. Next, the post-delivery process will be described. The relative position angle is determined by the relative angle measuring section 4 supplied, and the relative position is determined by the absolute angle measuring section 2 resupplied.

8-1. allegations

Measurements are made under the following conditions. The nth sample data (ie, the absolute position and the position measurements) are respectively denoted as follows:
x - _{g, n} , θ - _{g, n}

The measured values of the absolute position and the position can be simply written as x _{g, n} and θ _{g, n} .

The time at which the n-th sample data (absolute position and position) are detected is designated by t _{g, n} .

The mth sample data (ie, the absolute position and the position measurements) are respectively denoted as follows:
x - _{l, m} , θ - _{l, m}

The measured values of the absolute position and the position can be simply written as x _{l, m} and θ _{l, m} . The time at which the mth sample data (absolute position and position) is detected is designated by t1 _{, m} .

• Zustand 1: t_g,n ≤ t_l,m
• Zustand 2: t_g,n > t_l,m

Now consider the time at which the nth sample data (absolute position and position) is taken. It is assumed that the relative position and the position were sampled m times. Under these conditions, the following two states are possible:

• State 1: t _{g, n} ≦ t _{l, m}
• State 2: t _{g, n} > t _{l, m}

In state 1, the absolute position is measured at a time before an earlier relative position and position are measured. In state 2, newer information is obtained than corresponds to the previously measured relative position and position. Separately handling the two states increases accuracy. In state 1 there exists an integer k ≥ 0, which satisfies the following: t l, mk-1 <t g, n ≤ t l, mk

8-2. Add at t _{g, n} ≤ t _{l, m}

In this case can the relative position and the location can be easily replenished. It Different methods are available. For the present embodiment Three methods are described: (1) path measurement, (2) extended Displacement measurement and (3) linear subsequent delivery.

8-2-1. Subsequent delivery based on a distance measurement

In order to use displacement measurement, it is assumed that the attitude angular velocity ω _{l, mk-1} and the translation velocity v _{l, mk-1 are known} for the sampling data (mk-1).

Under these conditions, the relative position x _{l, n} , y _{l, n} , θ _{l, n} at t _{g, n} are rewritten as follows: x - l, n = x - l, mk-1 + (t g, n - t l, mk-1 ) v l, mk-1 cos (θ - l, mk-1 ) y - l, n = y - l, mk-1 + (t g, n - t l, mk-1 ) v l, mk-1 sin (θ - l, mk-1 ) θ - l, n = θ - l, mk-1 + (t g, n - t l, mk-1 ) ω l, mk-1

8-2-2. Subsequent delivery on the basis of an extended Displacement

No special information is required to use an extended path measurement. The subsequent delivery is made taking into consideration two cases: (1) when θ _{1, mk-1} = θ _{1, mk} , and (2) when θ _{1, mk-1} ≠ θ _{1, mk} holds.

If θ _{1, mk-1} = θ _{1, mk} , the subsequent delivery occurs in the following way:

If θ _{1, mk-1} ≠ θ _{1, mk} holds, the angle is first supplied as follows:

Then, for redelivering the position, the values r _{a, n} , r _{b, n} , and r _n defined in the manner defined below are introduced:

Here x _{l, n} and y _{l, n are written} as follows: x - l, n = r n sin (θ - l, n - θ - l, mk-1 ) cosθ - l, mk-1 - r n (1 - cos (θ - l, n - θ - l, mk-1 )) sinθ - l, mk-1 + x - l, mk-1 y - l, n = r n sin (θ - l, n - θ - l, mk-1 ) sin θ - l, mk-1 + r n (1 - cos (θ - l, n - θ - l, mk-1 ) cosθ - l, mk-1 + y - l, mk-1

8-2-3. Linear subsequent delivery

x _{l, n} , y _{l, n} , and θ _{l, n} , are supplied linearly based on the following equations:

8-3. Deliver, if t _{g, n} > t _{l, m}

This Subsection differs slightly from the previous one. It will four methods are described: (1) subsequent delivery based on a distance measurement, (2) subsequent delivery based on an extended distance measurement, (3) subsequent delivery, if the speed and the angular speed are not for disposal stand, and (4) linear subsequent delivery.

8-3-1. Subsequent delivery based on a distance measurement

To use a displacement measurement, it is assumed that the attitude angular velocity ω _{1, m} and the translation velocity v _{1, m are available} for the m-th sampling data.

Under these conditions, the relative position x _{1, n} , y _{1, n} , θ _{1, n are written} to t _{g, n} as follows: x - l, n = x - l, m + (t g, n - t l, m ) v l, m cos (θ - l, m ) y - l, n = y - l, m + (t g, n - t l, m ) v l, m sin (θ - l, m ) θ - l, n = θ - l, m + (t g, n - t l, m ) ω l, m

8-3-2. Subsequent delivery on the basis of an extended Displacement

To use extended path measurement, assume that the attitude angular velocity ω _{1, m} and the translation velocity v _{1, m are available} for the m-th sampling data, as above.

The angle is supplied in the following way: θ - l, n = θ - l, m + (t g, n - t l, m ) ω l, m

Then a variable δ _{l, n is} introduced: δ l, n = ω l, m (t g, n - t l, m )

• 1. when δ_l,n ≠ 0
  
• 2. when δ_l,n = 0 x -l,n = x -l,m-k-1 + (tg,n – tl,m-k-1)vl,m-k-1 cos(θ -l,m-k-1) y -l,n = y -l,m-k-1 + (tg,n – tl,m-k-1)vl,m-k-1 sin(θ -l,m-k-1)

Under these conditions x _{l, n} and y _{l, n} are calculated as follows:

• 1. when δ _{l, n} ≠ 0
  
• 2. when δ _{l, n} = 0 x - l, n = x - l, mk-1 + (t g, n - t l, mk-1 ) v l, mk-1 cos (θ - l, mk-1 ) y - l, n = y - l, mk-1 + (t g, n - t l, mk-1 ) v l, mk-1 sin (θ - l, mk-1 )

8-3-3. The speed and the angular speed Unavailable

The subsequent delivery is made taking into consideration two cases: (1) when θ _{1, m-1} = θ _{1, m} , and (2) when θ _{1, m-1} ≠ θ _{1, m} holds.

If θ _{1, m-1} = θ _{1, m} , then x _{1, n} , y _{1, n} , and θ _{1, n are} replenished based on the following equations:

If θ _{1, m-1} ≠ θ _{1, m} , the angle is first provided as follows:

Then, the values r _{a, n} , r _{b, n} , and r _n defined below are introduced to deliver the position:

Here x _{l, n} and y _{l, n} are written as follows: x - l, n = r n sin (θ - l, n - θ - l, m-1 ) cosθ - l, m-1 - r n (1 - cos (θ - l, n - θ - l, m-1 )) sinθ - l, m-1 + x - l, m-1 y - l, n = r n sin (θ - l, n - θ - l, m-1 ) sin θ - l, m-1 + r n (1 - cos (θ - l, n - θ - l, m-1 ) cosθ - l, m-1 + y - l, m-1

8-3-4. Linear subsequent delivery

Linear replenishment is based on the following equations:

9. It is not an absolute angle measuring section available

The absolute angle measuring section 5 is in the state estimator 1 In the present embodiment, it is not essential, in other words, the state estimator 1 can the initial state of the vehicle even without the absolute angle measuring section 5 estimated. Details follow.

First, it is assumed that the vehicle in the global coordinate system (x _{g, nk,} y _{g, nk)} to (x _{g, n,} y _{g n)} and in the local coordinate system (x _{l, nk,} y _{l, nk)} after (x _{l, n} , y _{l, n} ) has moved, as in the 5 is shown. The relative attitude angle, and the absolute attitude angle, θ _{g, n} , of the vehicle after the movement are given as follows: θ l, n = arg ((x l, n - x l, nk ) + i (y l, n - y l, nk )) θ g, n = arg ((x g, n - x g, nk ) + i (y g, n - y g, nk ))

Rewriting the initial state observation measure given by the equation (5) makes it possible to calculate θ _o in the initial state: θ whether, n + 1 = θ whether n - k θ (θ g, n - θ l, n )

The required for θ _o parameters are (x _{g, nk,} y _{g, nk)} and (x _{g, n,} y _{g n).} These parameters are determined by the absolute angle measuring section 2 and the absolute position measuring section 3 received (see the 1 ). Meanwhile, those by the relative angle measuring section 4 and the absolute angle measuring section 5 is not used for calculating θ _o .

Accordingly, the relative angle measuring section 4 and the absolute angle measuring section 5 be omitted in estimating the attitude angle of the vehicle in the initial state. Without these sections, the initial angle estimating section is 6 still capable, θ _o to be calculated from (x _{g, nk,} y _{g, nk)} and (x _{g, n,} y _{g n).} To estimate the absolute attitude angle of the vehicle in the current state, however, is the relative angle measuring section 4 required because θ _{l is} required in equations (1).

If the vehicle is not moving, in other words, if (x _{1, n} -x _{1, nk} , y _{1, n} -y _{1, nk} ) = (0, 0) and (x _{g, n} -x _{g, nk} , y _{g, n} -y _{g, nk} ) = (0, 0), θ _{1, n} and θ _{g, n} can not be calculated by the above process; however, the current position can be estimated. The accuracy of the estimation can be improved by updating the initial position estimating section and the absolute angle measuring section as follows: x whether, n + 1 = T (θ whether n ) x l + x whether n x l = 0

10. Verification attempts

Experiments were conducted in which a two-wheeled mobile robot was controlled to increase the effectiveness of the state estimator 1 to verify. The experiments used a modified displacement measurement as dead reckoning technique as well as a positioning system based on a CCD camera as sky navigation technology.

10-1. Devices used in experiments

The 6 shows the configuration of the system used in the experiments. The system contained a two-wheeled, mobile robot as a vehicle. The movement of the robot was controlled using a personal computer and measured with a vision system (image processor and CCD cameras).

The 7 (a) and 7 (b) show the configuration of the two-wheeled, mobile robot. The two-wheel mobile robot had a DC motor and an angle encoder attached to each of the two wheels so that the angles of the wheels could be independently measured. The two-wheeled mobile robot was controlled via a desktop PC. In the experiments, one scan was performed approximately every 9 ms.

The vision system used in the experiments performed a scan approximately every 17 ms. In addition, since the vision system could not directly measure the attitude angle of the robot, it did the same to measure the positions of two points, as in FIG 8th and calculated the attitude angle using the following equations:

Table 1 shows mean measurement errors and error distributions at the origin of the vision system. Table 1 Mean error and error distribution (number in the data set = 5000) x (cm) y (cm) θ (rad) Medium error -0.1716 0.015433 0.017922 error distribution 0.00133 0.000201 0.000986

10-2. experimental procedures

The movement of the two-wheeled mobile robot became from an initial state (x cm, y cm, θ rad) = (5.00, -10.00, 0.00) to a target state (x _r cm, y _r cm, θ _r rad) = (0.00, 0.00, 0.00). The tax rules used homogeneous, finite-term tax rules. The control lasted 180 seconds.

The initial state values of the two-wheel mobile robot measured with the vision system were used as initial values for the initial state observation measures. The gains k _x , k _y and k _θ in the observations were 0.0007, 0.0007 and 0.0007. The final arrival point (end point) estimates of the two-wheeled mobile robot made by the state estimating apparatus of the present embodiment and the actual measured values for the point obtained with a fine caliper and a protractor were recorded.

at The experiments were the initial state observation measures updated synchronously with the scan by the vision system. The Update an estimate theoretically requires local and global information, which is the same Time to be collected: the estimates of the monitoring activities are affected if, as in the current experiments, scanning with different intervals between the sky navigation technology and the dead reckoning technique. For the sake of simplicity Let us assume that the negative influence of the different sampling intervals is sufficiently small in the experiments. There was no special Correction.

10-3. test results

The 9 shows estimates of the global information and the 10 to 12 show estimates for the initial state. Table 2 shows estimates and actual values of global information for the Endpoint. Table 3 shows mean differences (errors) between the actual and the estimated values as well as error distributions. Table 2 test results estimated value No. x [cm] y [cm] θ [rad] 1 0.210 0,020 0.019 2 0,100 -0.020 0,008 3 0,160 0,010 0,003 4 0.130 0,000 0.028 5 0,100 -0.010 0.032 Measured value No. x [cm] y [cm] θ [rad] 1 -0.160 0,030 0,035 2 -0.160 0,050 0,035 3 -0.170 0,030 0,017 4 -0.140 0,000 0,017 5 -0.170 0,110 0,017 Table 3 Mean error and distribution between measured values and estimated values x (cm) y (cm) θ (rad) Medium error -0.3000 0.0440 0.0066 error distribution 0.0023 0.0025 0.0003

The 10 shows that x _{ob, n} converged to 5.12 cm about 80 seconds after the start of the control and started to converge to another value about 120 seconds after the start. The 9 indicates that the robot moved again about 120 seconds after the beginning, which is likely to indicate a sideways slide or other external disturbance experienced by the robot during the movement. The same discussion holds for y _{ob, n} and θ _{ob, n} .

11. Additional discussion

Finally, the blocks of the state estimator 1 , in particular, the initial angle estimating section 6 , the current-angle estimation section 7 , the initial position estimation section 8th and the current position estimating section 9 , be realized by hardware or software executed by a CPU as follows.

The state estimator 1 has a CPU (central processing unit) and storage devices (storage media). The CPU executes instructions contained in the functions implementing control programs. The storage devices may be a ROM (Read Only Memory) containing programs, a RAM (Random Access Memory) in which the programs are loaded, or a memory containing the programs and various data. The object of the invention can also be achieved by using the state estimator 1 a computer-readable storage medium attached which contains control program code (executable programs, intermediate code programs, or source programs) for the state estimator, which is software that realizes the above-mentioned functions for the computer (or the CPU, an MPU) to retrieve and execute the program code contained in the storage medium can.

Of the storage medium For example, a tape such as a magnetic tape or a cassette tape; a magnetic disk such as a floppy disk or a hard disk; or an optical disc such as a CDROM / MO / MD / DVD / CDR; a map, such as an IC card (memory card) or an optical card; or a semiconductor memory such as a mask ROM / EPROM / EEPROM / Flash ROM, be.

The state estimator 1 may be constructed so that it is connectable to a communication network, so that the program code can be delivered via this. The communication network is not limited in any specific way, and may include, for example, the Internet, an intranet, an extranet, a LAN, ISDN, VAN, a CATV communication network, a specific virtual network (virtual private network), a telephone line network, a mobile communication network or a satellite communications network. The transfer medium constituting the communication network is not limited in any specific way, and may be, for example, a wireline such as IEEE 1394, USB, an electric power line, a cable television line, a telephone line or DSL; or wireless, such as infrared radiation (IrDA, remote control), Bluetooth ^®, 802.11 radio, HDR, a mobile telephone network, a satellite line or a terestrisches digital network. The invention includes transmission by means of a carrier wave or a data signal, wherein the program code is electronically embedded.

The Invention shows a feature that it is the current position and can estimate the attitude angle of a vehicle without the measurement error being influenced by the relative attitude angle or position even if the initial position or the initial one Position angle of the vehicle is unknown. In addition, the invention requires, deviating techniques based on a Kalman filter, not a measurement error distribution matrix; the Invention is easily applicable even when it is difficult to get the error distribution.

The Zustandsabschätzvorrichtung according to the invention is preferably such that when the initial angle estimator the absolute position angle measures, the relative position measuring device the already measured relative attitude angle nachliefert and this the initial angle estimator outputs.

Different said, it is preferred if the initial angle estimator the initial one Lagewinkel appraises, over the Values of the relative and the absolute attitude angle of the vehicle to dispose of recorded at the same time. However, in general said, no relative attitude angle would be available if the initial attitude angle estimate is because the absolute angle measuring device the absolute attitude angle with a longer one Cycle measures the relative angle as the relative angle measuring device Position angle measures.

Accordingly, In the above configuration, the relative angle measuring device is the relined relative one Position angle to the initial angle estimator off. Accordingly, the Device even if the relative angle measuring device does not measure the relative attitude angle when the initial attitude angle estimate is, this using the supplied relative attitude angle instead of a measured value of the same estimate.

The Zustandsabschätzvorrichtung according to the invention is preferably such that when the absolute position measuring device the absolute position measures, the relative position measuring device the already measured Relativposition nachliefert and these subsequently delivered Output relative position to the initial position estimator.

Different That is, when the initial position estimating means is the initial position appraises, preferably, over the Values of the relative and absolute positions of the vehicle used for the same date. However, in general said, no relative position will be available when the initial position estimate is because the absolute position measuring device is the absolute position with a longer one Cycle measures, as the relative position measuring device, the relative position measures.

Accordingly, in the above configuration, the relative position measuring means outputs the relayed relative position to the initial position estimating means. Accordingly, the device itself when the relative position measuring means does not measure the relative position when the initial position is to be estimated, estimating it using the supplied relative position instead of a measured value thereof.

Some Steps of the Estimation Procedure can be implemented on a computer that a state estimation program is processed. This is how the computer achieves the same functions and effects as the state estimator according to the invention. By storing the state estimator in a computer readable storage medium The program can be executed on any computer.

INDUSTRIAL APPLICABILITY

The Invention can determine the current position and the attitude angle of a motor vehicle, of a ship or other vehicle even if the initial position and the initial one Position angles are unknown.

SUMMARY

There is provided a state estimator, method, computer program, and computer readable storage medium that does not rely on an error distribution matrix, but is still useful when the initial state is unknown. There are a current-angle estimation section ( 6 ), a current-angle estimation section ( 7 ), a current position estimating section ( 8th ) and a current position estimating section ( 9 ) provided. The current-angle estimation section ( 6 ) estimates the attitude angle (initial attitude angle) of a vehicle from a through a relative angle measurement portion (FIG. 4 ) and a through an absolute angle measuring section ( 5 ) obtained absolute position angle. The current-angle estimation section ( 7 ) estimates the attitude angle (current attitude angle) of the vehicle from the relative attitude angle and the initial attitude angle. The current position estimation section ( 8th ) estimates the position of the vehicle (initial position) from the initial attitude angle determined by a relative position measurement portion ( 2 ) and a relative position of the vehicle obtained by an absolute position measuring section ( 3 ) absolute position obtained. The current position estimation section ( 9 ) estimates the current position of the vehicle from the relative position, the estimated initial attitude angle, and the estimated initial position. Accordingly, the current position and the current attitude angle of the vehicle are estimated.

1

Zustandsabschätzvorrichtung

2

Absolute angle measurement section (Relative position measuring means)

3

Absolute position measurement section (Absolute position-measuring device)

4

Relative angle measuring section (Relative angle-measuring device)

5

Absolute angle measurement section (Absolute angle-measuring device)

6

Starting angle estimating (Initial angle estimator)

7

Current angle estimating (Current-angle estimator)

8th

Initial position estimating (Initial position estimator)

9

Current position estimating (Current position estimation)

Claims (8)

A state estimating apparatus comprising: a relative position measuring device for measuring, as a relative position, the position of a vehicle in a location coordinate system whose coordinate axes change with a movement of the vehicle; an absolute position measuring device for measuring, as an absolute position, the position of the vehicle in a global coordinate system whose coordinate axes are fixed independently of the movement of the vehicle; relative angle measuring means for measuring, as a relative attitude angle, the angle of the vehicle with respect to one of the coordinate axes of the location coordinate system; initial angle estimating means for calculating, as absolute attitude angle, the angle of the vehicle with respect to one of the coordinate axes of the global coordinate system from the relative position and the absolute position, and also estimating an initial attitude angle which is the attitude angle of the vehicle in its initial state in the global coordinate system, assuming that that a sequence represented by a recursive formula containing the relative attitude angle and the absolute attitude angle converges to a value corresponding to the initial attitude angle; current angle estimation means for estimating, as a sum of the relative attitude angle and the estimated initial attitude angle, the current attitude angle which is the attitude angle of the vehicle in its current state in the global coordinate system; an initial position estimating means for estimating the initial position which is the position of the vehicle in its initial state in the global coordinate system on the assumption that a sequence represented by a recursive formula containing the estimated initial attitude angle, the relative position and the absolute position , converges to a value corresponding to the initial position; and current position estimation means for estimating the current position which is the position of the vehicle in its current state in the global coordinate system based on the relative position, the estimated initial attitude angle and the estimated initial position. Zustandsabschätzvorrichtung With: a relative position measuring device for measuring, as Relative position, the position of a vehicle in a location coordinate system, whose coordinate axes change with a movement of the vehicle; one Absolute position measuring device for measuring, as absolute position, the position of the vehicle in a global coordinate system whose Coordinate axes independent are fixed by the movement of the vehicle; a relative angle measuring device for measuring, as a relative attitude angle, the angle of the vehicle with respect to one of the coordinate axes of the location coordinate system; one Absolute-angle measuring device for measuring, as the absolute position angle, the angle of the vehicle with respect to one of the coordinate axes the global coordinate system; an initial angle estimator to estimate an initial one Attitude angle, the attitude angle of the vehicle in its initial state in the global coordinate system, assuming that a sequence, which is represented by a recursive formula that expresses the relative Position angle and the absolute attitude angle contains, on a the initial Position angle corresponding value converges; a current-angle estimator to assess, as the sum of the relative attitude angle and the estimated initial one Position angle, the current attitude angle, which is the attitude angle of the Vehicle in its current state in the global coordinate system acting; an initial position estimator for estimating the Initial position, which is the position of the vehicle in its initial state in the global coordinate system, assuming that a sequence, which is represented by a recursive formula which is the estimated initial one Position angle, the relative position and the absolute position contains on converges a value corresponding to the initial position; and one Current position estimator to estimate the current position, which is the position of the vehicle in its current state in the global coordinate system is on basis the relative position, the estimated initial Attitude angle and the estimated Initial position. Zustandsabschätzvorrichtung according to claim 2, wherein when the absolute angle measuring device the absolute position angle measures, the relative angle measuring device the already measured relative attitude angle nachliefert and this the current-angle estimator outputs. Zustandsabschätzvorrichtung according to one of the claims 1 and 2, in which when the absolute angle measuring device the Absolute position measures, the relative position measuring device already measured relative position and delivers it to the current position estimator outputs. A state estimation method comprising the steps of: (1) measuring, as a relative position, the position of the vehicle in a location coordinate system whose coordinate axes change with movement of the vehicle; (2) measuring, as an absolute position, the position of the vehicle in a global coordinate system whose coordinate axes are fixed irrespective of the movement of the vehicle; (3) measuring, as a relative attitude angle, the angle of the vehicle with respect to one of the coordinate axes of the location coordinate system; (4) Calculating, as the absolute attitude angle, the angle of the vehicle with respect to one of the coordinate axes of the global coordinate system from the relative position and the absolute position, and also for estimating an initial attitude angle which is the attitude angle of the vehicle in its initial state in the global coordinate system on the assumption in that a sequence represented by a recursive formula containing the relative attitude angle and the absolute attitude angle converges to a value corresponding to the initial attitude angle; (5) Estimate, as the sum of the relative attitude angle and the estimated initial attitude angle, the current attitude angle, which is the attitude angle of the vehicle in its current state in the global coordinate system; (6) estimating an initial position which is the position of the vehicle in its initial state in the global coordinate system on the assumption that a sequence represented by a recursive formula including the estimated initial attitude angle, the relative position and the absolute position is set to one the value corresponding to the initial position converges; and (7) estimating the current position, which is the position of the vehicle in its current state in the global coordinate system, based on the relative position, the estimated initial attitude angle, and the estimated initial position. Zustandsabschätzverfahren with the following steps: (1) measuring, as a relative position, the position of the vehicle in a location coordinate system whose Coordinate axes change with a movement of the vehicle; (2) Measuring, as absolute position, the position of the vehicle in one Global coordinate system whose coordinate axes are independent of the movement of the vehicle are fixed; (3) Measure, as relative Attitude angle, the angle of the vehicle with respect to one of the coordinate axes of the Local coordinate system; (4) measuring, as the absolute positional angle, the angle of the vehicle with respect to one of the coordinate axes of the Global coordinate system; (5) Estimate an initial one Position angle, the attitude angle of the vehicle in its initial state in Global coordinate system is, assuming that a sequence, which is represented by a recursive formula that expresses the relative Position angle and the absolute attitude angle contains, on a the initial attitude angle corresponding value converges; (6) Estimate, as the sum of the relative Position angle and the estimated initial Position angle, the current attitude angle, the attitude angle of the vehicle is in its current state in the global coordinate system; (7) estimate an initial position showing the position of the vehicle in its Initial state in the global coordinate system is, assuming that a sequence represented by a recursive formula the estimated ones initial Position angle, the relative position and the absolute position contains on converges a value corresponding to the initial position; and (8th) estimate the current position, which is the position of the vehicle in its current state in the global coordinate system is on basis the relative position, the estimated initial Attitude angle and the estimated Initial position. Zustandsabschätz computer program, that allows a computer to one of the steps (4) to (7) of the method set forth in claim 5 or one of the steps (5) to (8) of the claim set forth in claim 6 Execute procedure. Computer-readable storage medium, the in claim 7 contained in the program.