JP2009198279A - On-vehicle navigation system and navigation method - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide an on-vehicle navigation system and a navigation method, capable of controlling even a system that has an optional sensor constitution by the same software as that of a basic system, in order to attain development efficiency and accuracy. <P>SOLUTION: In this on-vehicle navigation system, having a plurality of composite sensors including a vehicle speed pulse sensor, an accelerometer, an angular velocity meter (gyroscope) and a GPS receiver, the difference between a prescribed sensor constitution system selected from among a plurality of systems, constituted by a combination of some sensors and the basic system having all sensors is complemented; autonomous navigation calculation is performed, by using the complemented value and a sensor output signal of the system; and Kalman filter processing is performed, based on a control algorithm of the basic system, to thereby acquire estimation values of the position, the speed and the attitude of a vehicle. <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to an in-vehicle navigation system and a navigation method, and in particular, allows an algorithm of a basic system with 6 degrees of freedom to be used in common in an arbitrary sensor configuration system, and has a great accuracy even if the number of sensor axes is reduced. The present invention relates to an in-vehicle navigation system and a navigation method that do not decrease.

  The autonomous navigation system includes a vehicle speed pulse sensor that is a distance sensor of a vehicle, and inertia sensors such as an accelerometer and an angular velocity meter, and is widely used for estimating the position, speed, and posture of a moving vehicle.

  The Inertial Navigation System (INS) is usually composed of a 3-axis accelerometer and a 3-axis gyroscope, and is used to simultaneously estimate the 6 degrees of freedom of a moving object, that is, 3D translational motion and 3D rotational motion. Is done. For example, navigation of an aerospace vehicle uses such an inertial navigation system to simultaneously estimate six degrees of freedom of a moving object, that is, three-dimensional translational motion and three-dimensional rotational motion. In vehicle navigation, the use of a vehicle speed pulse sensor often reduces costly inertial sensors.

  Autonomous navigation can update the estimated position based on the sensor output without relying on an external signal, but errors such as noise contained in the sensor output accumulate with the integration calculation over time. Cannot be used. Therefore, by combining the Global Positioning System (GPS receiver) and the autonomous navigation system, which always provide finite error position estimation within the reach of satellite signals, with an optimal probability algorithm such as Kalman filter, A technique for interpolating and solving performance problems has been devised (Patent Document 1). Such a system is called an integrated navigation system and is now widely used from vehicle navigation to aerospace navigation.

In recent vehicle navigation, there are navigation systems with various sensor configurations ranging from low-cost products using a single GPS receiver to high-end products using a combination of a GPS receiver and an arbitrary sensor. However, when one company handles low-cost products to high-end products, the combination of sensors in each product is different, and it is usually necessary to develop different software (SW) for each product. There was a problem of increasing in proportion to the number.
Japanese Patent No. 3473117

  Therefore, there is a need for a navigation method and an in-vehicle navigation system that has a single platform software (SW) that can be commonly used for a plurality of systems with different sensor combinations, and that does not significantly reduce accuracy even if the number of sensor axes decreases. ing.

An object of the present invention is to enable a single platform software (SW) to support a plurality of systems having different combinations of sensors, and to prevent a large decrease in accuracy even if the number of sensor axes is reduced.
Another object of the present invention is to make the 6-DOF basic system algorithm commonly available in any sensor configuration system.

The present invention is an in-vehicle navigation system and a navigation method in which a vehicle speed pulse sensor, an accelerometer, an angular velocity meter (gyroscope), and a plurality of sensors of a GPS receiver are combined.
Navigation method The navigation method of the present invention includes a step of selecting a predetermined sensor configuration system from a plurality of systems configured by a combination of several sensors, and a basic configuration including the selected sensor configuration system and all sensors. Complementing a difference from the system, and performing a Kalman filter process based on the control algorithm of the basic system to obtain an estimated value of the position, speed and posture of the vehicle.
The sensor configuration system is at least
GPS receiver, vehicle speed pulse sensor, accelerometer 3-axis, gyroscope 3-axis,
GPS receiver, vehicle speed pulse sensor, accelerometer 3 axis, gyroscope 2 axis,
GPS receiver, vehicle speed pulse sensor, 3 axes of accelerometer, 1 axis of gyroscope,
GPS receiver, vehicle speed pulse sensor, 2 axes of accelerometer, 1 axis of gyroscope,
GPS receiver, vehicle speed pulse sensor, accelerometer 1 axis, gyroscope 1 axis,
GPS receiver, vehicle speed pulse sensor, accelerometer not used (0 axis), gyroscope 1 axis,
The basic system is a six-degree-of-freedom system including a GPS receiver, a vehicle speed pulse sensor, three axes of accelerometers, and three axes of gyroscopes.
The navigation method of the present invention further includes a step of presetting the selected sensor configuration system at the time of system design or automatically determining based on a signal obtained from the sensor.
In the navigation method of the present invention, the complementing means for complementing the difference employs 0 or white noise as the output value of the sensor that is missing from the basic system in the selected sensor configuration system. Alternatively, the complement means employs a constant as the output value of the sensor that is missing from the basic system in the selected sensor configuration system. Alternatively, the complement means adopts a function value of a function having the output value of another sensor as a variable as the output value of the sensor that is missing from the basic system in the selected sensor configuration system.

In-vehicle navigation system The in-vehicle navigation system of the present invention is a complement that complements the difference between a predetermined sensor configuration system selected from a plurality of systems configured by a combination of several sensors and a basic system including all sensors. An autonomous navigation calculation unit that performs autonomous navigation calculation using the complemented value and the sensor output signal of the system, performs a Kalman filter process using the autonomous navigation calculation result and the GPS measurement result based on the control algorithm of the basic system, A processing unit that obtains estimated values of the position, speed, and posture. The sensor configuration system can have a plurality of configurations shown in the navigation method, and the basic system is a six-degree-of-freedom system including a GPS receiver, a vehicle speed pulse sensor, three accelerometer axes, and three gyroscope axes. It is.
The vehicle-mounted navigation system of the present invention further includes means for presetting the selected sensor configuration system at the time of system design, or means for automatically determining the selected sensor configuration system based on a signal obtained from the sensor. It has.
In the in-vehicle navigation system of the present invention, the complementing unit adopts 0 or white noise as an output value of a sensor that is missing from the basic system in the selected sensor configuration system. Alternatively, the complementing unit employs a constant as the output value of the sensor that is missing from the basic system in the selected sensor configuration system. Or a complement part employ | adopts the function value of the function which makes the output value of another sensor a variable as an output value of the sensor missing from the basic system in the said selected sensor structure system.

  According to the present invention, according to a predefined sensor configuration, by adopting a predetermined value based on dynamic knowledge as an output value of a sensor that is missing from the basic six-degree-of-freedom system, autonomous navigation The calculation unit can always calculate the same basic system with 6 degrees of freedom. As a result, the same software (SW) can be used for different sensor configuration systems, reducing development man-hours. Also, in the present invention, more information is used for the sensor signal that is dynamically estimated and supplemented than an algorithm that only supports an individual sensor configuration system, thereby improving the accuracy of autonomous navigation calculation. For this reason, according to the present invention, even if the number of sensor axes is reduced, the decrease in accuracy is small as compared with an algorithm corresponding only to an individual sensor configuration system.

(A) Basic Configuration of Composite Navigation System FIG. 1 is a schematic block diagram showing an example of a basic configuration of a composite navigation system of the present invention mounted on a vehicle.

  The input signal of the autonomous system is a signal obtained from an inertial sensor on the sensor board 10 and a vehicle speed pulse taken directly from the vehicle body into the navigation CPU through another cable. As an inertial sensor, an accelerometer (acceleration 1 to 3 axes) and a gyroscope (angular velocity 1 to 3 axes) are adopted. As a sensor for generating a vehicle speed pulse, one pulse is generated every time the vehicle moves a predetermined distance. Adopt a vehicle speed sensor.

  The sensor configuration (SC) controller 20 determines the sensor configuration by setting the actual sensor configuration of the in-vehicle navigation system from the setting unit 15 or evaluating information acquired from the sensor output. The evaluation of the information acquired from the sensor output refers to “when the sensor output signal is an abnormal value, the system automatically determines that the sensor configuration does not use the corresponding sensor”. The sensor output signal has an abnormal value means that, for example, (1) a constant voltage is constantly output, or (2) the bias or sensitivity exceeds a specification range.

  Depending on the determined sensor configuration, the SC controller 20 uses a constant (including 0) or white noise as the output value of the sensor missing from the basic system with 6 degrees of freedom, based on dynamic knowledge. Alternatively, the function value of a function having another sensor output value as a variable is adopted, and the output value of each sensor is sent to the autonomous navigation calculation unit 30. In addition, when the discriminated sensor configuration does not have a sensor for estimating the roll angle, the SC controller 20 uses the Kalman filter correction unit 40 to measure the measurement value of the roll angle = 0 in order to prevent the roll angle estimation amount from diverging. Send to. At this time, a roll angle of about 5 degrees at maximum occurs in an actual vehicle, but the Kalman filter correction unit 40 treats an error from the actual value as white noise.

  The autonomous navigation calculation unit 20 always updates the vehicle state quantity including the current position, speed, and posture of the vehicle at a high frequency by using the same basic system algorithm with six degrees of freedom.

  The GPS receiver 50 receives signals related to the distance and the rate of change in distance from a plurality of artificial satellites, thereby providing measurements of the position of the vehicle antenna and the vehicle speed.

  The calculation unit 60 takes the difference between the vehicle state quantity estimated by the autonomous navigation calculation unit 30 and the measurement quantity including the position / velocity measurement value obtained from the GPS receiver 50, and the Kalman filter correction unit 40 uses the Kalman filter. Based on the difference, the vehicle state quantity including the current position, speed, and posture of the vehicle is corrected.

(B) Sensor Configuration in Various Systems FIG. 2A shows a coordinate system Xs-Ys-Zs fixed to the sensor board 10 in FIG. It should be noted that the expression relating to the sensor (board) coordinate system is expressed by the subscript “s” in the following mathematical expressions.

  FIG. 2 (B) shows that the sensor board 10 of (A) is composed of three axes of accelerometers (Acc x, Acc y, Acc z) and three axes of gyroscopes (Gyro x, Gyro y, Gyro z). It shows the sensor configuration of the basic system of freedom. The three-axis accelerometers Acc x to Acc z detect acceleration in three coordinate directions (x, y, z) of the sensor coordinate system, and the three-axis gyroscopes Gyro x to Gyro z are three sensors of the sensor coordinate system. Angular velocities P, Q, and R around the coordinate axes (x, y, z) are detected.

  FIG. 2 (C) shows a sensor configuration of a system that includes three axes of accelerometers and two axes of gyroscopes in the sensor board 10 of (A). Here, when the gyroscope can only detect two axes, the gyroscope of the system is Gyro x and Gyro z.

  FIG. 2 (D) shows the sensor configuration of a system comprising three axes of accelerometers and one axis of gyroscope in the sensor board 10 of (A). Here, when the gyroscope can detect only one axis, the gyroscope of the system is Gyro z.

  FIG. 3 (A) shows the sensor configuration of a system comprising two axes of accelerometers and one axis of gyroscope in the sensor board 10 of FIG. 2 (A). Here, when the accelerometer can detect only two axes, the accelerometer of the system is Acc x, Acc y, and when the gyroscope can detect only one axis, the gyroscope of the system is Gyro z.

  FIG. 3B shows a sensor configuration of a system including one axis of accelerometer and one axis of gyroscope in the sensor board 10 of FIG. Here, when the accelerometer can detect only one axis, the system accelerometer is Acc x, and when the gyroscope can detect only one axis, the system gyroscope is Gyro z.

  Fig. 3 (C) shows the sensor configuration of the system consisting of the accelerometer 0 axis (not used) and the gyroscope 1 axis in the sensor board 10 of Fig. 2 (A). When not detectable, the system's gyroscope is Gyro z.

  FIG. 4A shows a coordinate system Xb-Yb-Zb fixed to the vehicle. In the following formula, the expression relating to the vehicle fixed coordinate system is expressed by the subscript “b”.

  FIG. 4B shows a surface fixed coordinate system North-East-Down (NED coordinate system) at a certain latitude and longitude. In the following formula, the expression related to the NED coordinate system is expressed by the subscript “n”.

(C) Basic Operation Process FIG. 5 is a flowchart showing an example of a basic operation process of the navigation system according to the present invention. In step 101, several sensor configurations are defined in advance based on a vehicle speed pulse sensor, an accelerometer that is an inertial sensor, and an angular velocity meter (see FIGS. 2 and 3).

  Next, in step 102, the SC controller 20 determines the sensor configuration of the navigation system based on the signal obtained from the sensor.

  In step 103, the SC controller 20 determines a constant (including 0) based on dynamic knowledge as an output value of the sensor missing from the basic system of 6 degrees of freedom, according to the determined sensor configuration. The function value of the function using white noise or another sensor output as a variable is adopted, and the output of each sensor is sent to the autonomous navigation calculation unit 30. In addition, the SC controller 20 sends a measurement amount of roll angle = 0 to the Kalman filter correction unit 40. In some cases, the measurement amount with the roll angle = 0 may not be sent to the Kalman filter correction unit 40.

  In step 104, the autonomous navigation calculation unit 30 uses the same six-degree-of-freedom basic system algorithm to constantly update the vehicle state quantity at high frequency based on the value adopted as the sensor output and the actual sensor output value. Perform navigation calculations.

  In step 105, the calculation unit 60 calculates the difference between the vehicle state quantity estimated by the autonomous navigation calculation unit 30 and the measurement quantity including the position / velocity measurement value obtained from the GPS receiver 50, and the Kalman filter correction unit 40 The vehicle state quantity including the current position, speed, and posture of the vehicle is corrected based on the difference using the Kalman filter.

  In step 106, the combined autonomous / GPS receiver navigation system iteratively executes steps 102 to 105 described above to continuously optimize navigation accuracy.

  In the following, a specific method for performing estimation calculation of the vehicle state quantity using the algorithm of the basic 6-degree-of-freedom system that is always based on the sensor configuration will be described.

で示すように変数の上にドットがついているものは、その変数ｖ_sの時間変化率を表しているものとする。 (D) Autonomous Navigation Calculation Algorithm in Autonomous Navigation Calculation Unit First, the algorithm of the high-frequency autonomous navigation calculation unit 30 of the basic six-degree-of-freedom system will be described. In the following formula,

Those with dot over the variable as shown, it is assumed that represents the time rate of change of the variable v _s.

で表現される。但し、

であり、(2a)はセンサー固定座標系で表現される速度ベクトル、(2b)はセンサー固定座標系で表現されるジャイロスコープ出力ベクトル、(2c)はセンサー固定座標系で表現される加速度計出力ベクトル,(2d)はセンサー固定座標系で表現される重力ベクトルである。ここで、

ここで、センサーの“感度”は、設置角の影響などを含めない、センサー自体の電圧出力から物理量への変換定数を意味する。通常センサーの生の出力は、出力がないことを示す０点電圧（しばしば２．５Ｖｏｌｔ）を中心とした電圧値（単位はＶｏｌｔ）であり、角度変化率や加速度などの物理量を得るためには、感度とよばれる変換用の定数を、出力電圧値と０点電圧の差に乗算する必要がある。感度は通常仕様値がセンサー毎に決められているが、低コストのセンサーの場合、実際の値が仕様値と大きく異なることや、温度補正が正確でないために感度が仕様値とずれるといったことがしばしば起こる。ことにジャイロに関する感度誤差はナビゲーションの方位計算に大きな影響を及ぼすので、このシステムではジャイロ感度をカルマンフィルタの状態量に含め、常時推定・補正を行う。 (A) Speed equation The speed equation is:

It is expressed by However,

(2a) is the velocity vector expressed in the sensor fixed coordinate system, (2b) is the gyroscope output vector expressed in the sensor fixed coordinate system, and (2c) is the accelerometer output expressed in the sensor fixed coordinate system. Vector, (2d) is the gravity vector expressed in sensor fixed coordinate system. here,

Here, the “sensitivity” of the sensor means a conversion constant from the voltage output of the sensor itself to a physical quantity without including the influence of the installation angle. Normally, the raw output of a sensor is a voltage value (unit is Volt) centered on a zero point voltage (often 2.5 Volt) indicating that there is no output, and in order to obtain a physical quantity such as an angle change rate and acceleration. It is necessary to multiply the difference between the output voltage value and the zero point voltage by a conversion constant called sensitivity. The sensitivity is usually specified for each sensor, but in the case of a low-cost sensor, the actual value may differ greatly from the specification value, or the sensitivity may deviate from the specification value due to inaccurate temperature correction. Often happens. In particular, the sensitivity error related to the gyro greatly affects the calculation of the navigation direction. In this system, the gyro sensitivity is included in the state quantity of the Kalman filter and is always estimated and corrected.

ただし、

はセンサー座標系からNED座標系への変換行列である。
（4）式は、慣性航法システム（Inertial Navigation System: INS）の一般的な手法である以下の姿勢方程式

から、車両ナビゲーション適用の際に独立な4つのパラメータだけを抜き出したものである。(5)式はより一般的なINS姿勢方程式である。 (B) Posture equation The posture equation is expressed by the following equation.

However,

Is a transformation matrix from the sensor coordinate system to the NED coordinate system.
Equation (4) is the following attitude equation, which is a general method of the inertial navigation system (INS).

From the above, only four independent parameters are extracted when applying vehicle navigation. Equation (5) is a more general INS attitude equation.

により表現できる。但し、(6)式において

であり、(7a)は位置ベクトル、(7b)は車両移動距離ベクトルである。又、(7b)式において、N_p はサンプル時間当たりのパルス数、Lはパルス間距離である。 (C) Position equation The position equation is

Can be expressed by However, in equation (6)

(7a) is a position vector, and (7b) is a vehicle movement distance vector. Further, in (7b) equation, N _p is the number of pulses per sample time, L is the pulse spacing.

により表現される。但し、

であり、(9a)はセンサー座標系ジャイロスコープ出力に含まれるバイアス（Volt）、(9b)はセンサー座標系加速度計出力に含まれるバイアス（m/s²）、(9c)はセンサー座標系ジャイロスコープ出力に関する感度（rad/sec/Volt）、(9d)はセンサー座標系から車両座標系への変換行列である。また、(8）式中p₀₀、p₁₀、及びp₂₀、は取り付け角に関する行列の要素から、独立な3つのパラメータだけを抜き出したものである。 (D) Fixed parameter equation The fixed parameter equation is

It is expressed by However,

(9a) is the bias (Volt) included in the sensor coordinate system gyroscope output, (9b) is the bias (m / s ² ) included in the sensor coordinate system accelerometer output, and (9c) is the sensor coordinate system gyro. Sensitivity (rad / sec / Volt) regarding scope output, (9d) is a transformation matrix from the sensor coordinate system to the vehicle coordinate system. In addition, p ₀₀ , p ₁₀ , and p _{20 in the} equation (8) are obtained by extracting only three independent parameters from the matrix elements related to the mounting angle.

但し、ｘは以下に与えられる非線形状態量ベクトルである。

(E) Nonlinear equation of state The above equations (1), (4), (6), and (8) can be put together and expressed simply as a determinant like equation (10).

Where x is a nonlinear state quantity vector given below.

を用いて行えばよい。精度要求の高い場合は、周知の数値積分方法であるルンゲ・クッタ4次式(下記文献参照)などを用いてもよい。
Kreyszig, E., Advanced Engineering Mathematics, John Wiley & Sons, 1999, New York, NY.
但し、(11)式において、Tはサンプルタイムで、例えば25HzならばT=0.04秒である。以上は一般的な慣性航法システム（INS）に用いられる手法を基に、MEMSセンサー使用を考慮して、地球の丸みなど微小な項を簡略化した車両用慣性航法システム（INS）である(下記文献参照)。
Hoshizaki, T., Computational Scheme for MEMS Inertial Navigation Systems, AOAMR Patent, August, 2006. (F) State quantity update
In order to numerically integrate equation (10) on the CPU, for example, the state quantity update equation

Can be used. When the accuracy requirement is high, the Runge-Kutta quartic equation (see the following document) which is a well-known numerical integration method may be used.
Kreyszig, E., Advanced Engineering Mathematics, John Wiley & Sons, 1999, New York, NY.
However, in the equation (11), T is a sample time. For example, if it is 25 Hz, T = 0.04 seconds. The above is the vehicle inertial navigation system (INS) based on the techniques used in general inertial navigation system (INS), with consideration of the use of MEMS sensors and the simplified terms such as the roundness of the earth. See literature).
Hoshizaki, T., Computational Scheme for MEMS Inertial Navigation Systems, AOAMR Patent, August, 2006.

但し、

は以下に与えられる微小擾乱ベクトルである。

ただし、(13b)は

の現在推定値、(13c)はホワイトノイズでモデル化された3軸ジャイロスコープ出力

に含まれる高周波ノイズと、3軸加速度計出力

に含まれる高周波ノイズである。それぞれのノイズの標準偏差(σ）はセンサースペックや、測定実験などによって得られ、ここではそれらの大きさを次のようにＮで表す。

また、

に関する微小擾乱量であり、

には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。

また、

に関する微小擾乱量であり、

には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。

ここで車両に対するセンサーの取り付けロール角＝0と仮定すれば、取り付けロール角に関する補正量

となる。
(12)式における線形システムを表わす行列

はそれぞれ図６、図７に示すように得られる。ただし、図６において、

(G) Micro disturbance equation Now, in order for the Kalman filter correction unit 40 to correct the state quantity x by the Kalman filter processing based on the measured value of the GPS receiver or the like, the autonomous navigation calculation unit 30 converts the state quantity into a non-linear state. It is necessary to update by the equation and also update the covariance value of the error amount (small disturbance amount) on the assumption that the error amount increases by the linear equation. For this purpose, the equation (11) is linearized to prepare the determinant (12) which is a small disturbance equation.

However,

Is the small disturbance vector given below.

However, (13b)

Current estimate, (13c) is a 3-axis gyroscope output modeled with white noise

High frequency noise and 3-axis accelerometer output

Is high frequency noise. The standard deviation (σ) of each noise is obtained by sensor specifications, measurement experiments, and the like, and here, the magnitude thereof is represented by N as follows.

Also,

The amount of minute disturbance

Has the following relationship based on the general method of inertial navigation system (INS).

Also,

The amount of minute disturbance

Has the following relationship based on the general method of inertial navigation system (INS).

Assuming that the mounting roll angle of the sensor with respect to the vehicle is 0, the correction amount for the mounting roll angle

It becomes.
Matrix representing linear system in Eq. (12)

Are obtained as shown in FIGS. 6 and 7, respectively. However, in FIG.

を使って、カルマンフィルタ処理の定式に従い、高周波で以下のように微小擾乱ベクトル

のコバリアンス行列(誤差共分散行列)を次式により更新する。

但し、右肩のT は転地行列（Transpose）を意味し、右肩の−はカルマンフィルタの補正前の状態を意味し、右肩の＋はカルマンフィルタの補正後の状態を意味する。また、

の相関値の期待値（E[ ]）からなるコバリアンス行列であり、

に関するコバリアンス行列であり、それぞれ次式で表現される。

以上の（11）式及び（18）式が、前記6自由度の基本システムにおいて自律航法計算部３０が高周波で(例えば25Hzで)実行する演算式である。以下に(11)式及び（18）式を再掲する。

(H) Updating the covariance matrix

And according to the Kalman filter processing formula,

The covariance matrix (error covariance matrix) is updated by the following equation.

However, T on the right shoulder means a transpose matrix (Transpose),-on the right shoulder means a state before correction of the Kalman filter, and + on the right shoulder means a state after correction of the Kalman filter. Also,

Is a covariance matrix consisting of the expected correlation value (E [])

The covariance matrix is expressed by the following equations.

The above formulas (11) and (18) are calculation formulas that the autonomous navigation calculation unit 30 executes at a high frequency (for example, at 25 Hz) in the basic system with six degrees of freedom. Equations (11) and (18) are shown again below.

(E) Method for determining output of sensor missing from basic system of arbitrary system Here, a method for determining the output value of a sensor missing from the basic system of 6 degrees of freedom in a predetermined sensor configuration system will be described. Based on dynamic knowledge, the SC controller 20 adopts a constant, white noise, or a function value of a function having other sensor output as a variable as an output value of the sensor that is missing from the basic system. Input to the navigation calculation unit 30. Further, the SC controller 20 inputs the measurement amount of the roll angle = 0 to the Kalman filter correction unit 40.

(A) Basic system with vehicle speed pulse sensor, accelerometer 3 axes, and gyroscope 3 axes :
In the case of such a basic system, the SC controller 20 inputs each sensor output to the autonomous navigation calculation unit 30, and the autonomous navigation calculation unit 30 executes the autonomous navigation calculation algorithm using the input sensor output ((11), (18) Perform the calculation of the equation).

とする。即ち、車両モーションによるピッチ角速度を、平均値0（rad/s）と標準偏差0.05（rad/s）＝2.9（deg/s）のホワイトノイズの加算であると仮定する。標準偏差値は6自由度の基本システムを用いて得られたデータを統計処理した値を使用している。後述する図８の螺旋駐車場を含むテストコースを実際に走行して採取したｙ_s軸ジャイロスコープ出力データの標準偏差値は0.054 （rad/s）であり、上記の標準偏差0.05（rad/s）は乗用車の車両ピッチ角モーションのおおよその代表的な値であると仮定できる。
（3）ｙ_s軸のジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ

で固定する。 (b) System with vehicle speed pulse sensor, accelerometer 3-axis and gyroscope 2-axis:
In such a system, the sensor output is determined according to the following (1) to (3).
(1) The x _s and z _s axes are used for the two gyroscope axes (Fig. 2 (C)).
(2) y _s-axis gyroscope output and _{0 (ω ys = Q = 0} ),

And That is, it is assumed that the pitch angular velocity due to vehicle motion is an addition of white noise with an average value of 0 (rad / s) and a standard deviation of 0.05 (rad / s) = 2.9 (deg / s). Standard deviation values are obtained by statistically processing data obtained using a basic system with six degrees of freedom. The standard deviation value of the y _s- axis gyroscope output data obtained by actually running the test course including the spiral parking lot of FIG. 8 described later is 0.054 (rad / s), and the above standard deviation of 0.05 (rad / s) ) Can be assumed to be an approximate representative value of the vehicle pitch angle motion of a passenger car.
(3) The bias and sensitivity included in the y _s axis gyroscope output

Secure with.

とする。即ち、車両モーションによるロール角速度の大きさを平均値0（rad/s）と、標準偏差0.05（rad/s）＝2.9（deg/s）のホワイトノイズの加算である仮定する。標準偏差値は6自由度の基本システムを用いて得られたデータを統計処理した値を使用している。後述する図９の螺旋駐車場を含むテストコースを実際に走行して採取したｘ_s軸ジャイロスコープ出力データの標準偏差値（0.049 rad/s）であり、上記の標準偏差0.05（rad/s）は乗用車の車両ロール角モーションのおおよその代表的な値であると仮定できる。
また、ｙ_s軸ジャイロスコープ出力を0（ω_ys=Q=0）とし、

とする。更に、
ｚ_s軸加速度計出力を‐9.8（m/s²）とし、

とする。即ち、車両モーションによるセンサー垂直方向加速度の大きさを平均値‐9.8（m/s²）と、 標準偏差0.3（m/s²）のホワイトノイズの加算であると仮定する。標準偏差値は6自由度システムを用いて得られたデータを統計処理した値を使用している。後述する図９の螺旋駐車場を含むテストコースを実際に走行して採取したｚ_s軸加速度計出力データの標準偏差値は0.28（m/s²）であり、上記の標準偏差0.3（m/s²）は乗用車の車両垂直方向加速度モーションのおおよその代表的な値であると仮定できる。
（3）ｘ_s軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ

で固定する。また、ｙ_s軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ

で固定する。さらに、z_s軸加速度計出力に含まれるバイアス

で固定する。 (c) A system with a vehicle speed pulse sensor, 3 axes of accelerometers and 1 axis of gyroscope , or a system with a vehicle speed pulse sensor, 2 axes of accelerometers and 1 axis of gyroscope:
In such a system, the sensor output is determined according to the following (1) to (3).
(1) The two axes of the accelerometer use the x _s and y _s axes. In addition, the z _s axis is used as the gyroscope 1 axis (FIGS. 2D and 3A).
(2) x _s-axis gyroscope output and _{0 (ω xs = P = 0} ),

And That is, the roll angular velocity due to the vehicle motion is assumed to be an addition of white noise with an average value of 0 (rad / s) and a standard deviation of 0.05 (rad / s) = 2.9 (deg / s). Standard deviation values are obtained by statistically processing data obtained using a basic system with six degrees of freedom. This is the standard deviation value (0.049 rad / s) of the x _s axis gyroscope output data collected by actually running the test course including the spiral parking lot of FIG. 9 to be described later, and the above standard deviation of 0.05 (rad / s) Can be assumed to be an approximate representative value of the vehicle roll angular motion of a passenger car.
Also, set the y _s axis gyroscope output to 0 (ω _ys = Q = 0),

And Furthermore,
z _s- axis accelerometer output is -9.8 (m / s ² )

And In other words, the magnitude of the sensor vertical acceleration due to vehicle motion is assumed to be the addition of white noise with an average value of −9.8 (m / s ² ) and a standard deviation of 0.3 (m / s ² ). The standard deviation value is the value obtained by statistically processing the data obtained using the 6-DOF system. The standard deviation value of the output data of the z _s- axis accelerometer collected by actually running the test course including the spiral parking lot in FIG. 9 described later is 0.28 (m / s ² ), and the standard deviation value is 0.3 (m / s). It can be assumed that s ² ) is an approximate representative value of the vehicle vertical acceleration motion of a passenger car.
(3) Bias and sensitivity included in _xs axis gyroscope output

Secure with. Further, each of the bias and sensitivity contained in y _s axis gyroscope output

Secure with. In addition, the bias included in the z _s- axis accelerometer output

Secure with.

とする。また、ｙ_s軸ジャイロスコープ出力を0（ω_ys=Q=0）とし、

とする。更に、ys軸加速度計出力を、
車速パルスから得られる速度（ｖ_xb_obs）×ｚ_s軸ジャイロスコープ出力ω_zs（=R)
とする（掛け算の結果の単位はm/s²）。速度ｖ_xb_obsについては後述するが、ys軸加速度計出力は、ｚ_s軸ジャイロスコープ出力ω_zsを変数とする関数の関数値であるといえる。また

とする。即ち、車両モーションによるセンサー横方向加速度の大きさを、ｖ_xb_obs×ω_zsの平均値と標準偏差0.7（m/s²）のホワイトノイズの加算であると仮定する。平均値を表す上式は、力学的に平面内回転運動において、横方向加速度が前方向速度と角速度の積で得られることから、定性的に推定されたものである。それ以外の横方向加速度出力はロール角による重力方向成分を含む。標準偏差値は6自由度システムを用いて得られたデータを統計処理した値を使用している。後述する図９の螺旋駐車場を含むテストコースを実際に走行して採取したｙ_s軸加速度計出力データとｖ_xb_obs×ω_zsとの差の標準偏差値は0.72（m/s²）であり、上記の標準偏差0.7（m/s²）は、乗用車の横方向加速度モーションのおおよその代表的なモデルと仮定できる。また、(ｃ)のシステムと同様にｚ_s軸加速度計出力を‐9.8（m/s²）とし、N_wazs=0.3（m/s²）とする。
（3）ｘ_s軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ

で固定する。また、ｙ_s軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ

で固定する。さらに、ｙ_s軸加速度計、z_s軸加速度計出力に含まれるバイアス

で固定する。
（4）ロール角＝0の測定量を後述のカルマンフィルタ補正部４０で使用する。 (D) Vehicle speed pulse sensor, accelerometer single axis, and gyroscope single axis system:
In such a system, the sensor output is determined according to the following (1) to (4).
(1) One accelerometer axis shall be the x _s axis, and one gyroscope axis shall be the z _s axis (see FIG. 3B).
(2) System and similarly x _s-axis gyroscope output (c) and _{0 (ω xs = P = 0} ),

And Also, set the y _s axis gyroscope output to 0 (ω _ys = Q = 0),

And In addition, the ys-axis accelerometer output
Speed obtained from vehicle speed pulse (v _xb _obs) x z _s- axis gyroscope output ω _zs (= R)
(The unit of the result of multiplication is m / s ² ). Although the speed v _xb _obs will be described later, it can be said that the ys-axis accelerometer output is a function value of a function having the z _s- axis gyroscope output ω _zs as a variable. Also

And That is, it is assumed that the magnitude of the lateral acceleration of the sensor due to the vehicle motion is an addition of the average value of v _{xb —} obs × ω _zs and white noise with a standard deviation of 0.7 (m / s ² ). The above equation representing the average value is qualitatively estimated because the lateral acceleration is obtained by the product of the forward velocity and the angular velocity in the in-plane rotational motion. The other lateral acceleration output includes a gravity direction component due to the roll angle. The standard deviation value is the value obtained by statistically processing the data obtained using the 6-DOF system. The standard deviation of the difference between the y _s- axis accelerometer output data and v _xb _obs × ω _zs actually collected from the test course including the spiral parking lot of FIG. 9 described later is 0.72 (m / s ² ). Yes, the above standard deviation of 0.7 (m / s ² ) can be assumed to be an approximate representative model of the lateral acceleration motion of a passenger car. Similarly to the system (c), the z _s- axis accelerometer output is set to −9.8 (m / s ² ), and N _wazs = 0.3 (m / s ² ).
(3) Bias and sensitivity included in _xs axis gyroscope output

Secure with. Further, each of the bias and sensitivity contained in y _s axis gyroscope output

Secure with. In addition, bias included in y _s- axis accelerometer and z _s- axis accelerometer output

Secure with.
(4) The measured amount of roll angle = 0 is used in the Kalman filter correction unit 40 described later.

とし、ｙ_s軸ジャイロスコープ出力を0（ω_ys=Q=0）とし、

とする。また、ｘ_s軸加速度計出力を0とし、

とする。即ち、車両モーションによるセンサー前方向加速度の大きさを、平均値0（m/s²）と、 標準偏差0.8（m/s²）のホワイトノイズの加算であると仮定する。この標準偏差値は坂道の多さやドライバーの加減速によって0.5〜1.0（m/s²）と異なる値であるが、ここでは後述する図９の螺旋駐車場を含むテストコースを実際に走行して採取したｘ_s軸加速度計出力データの標準偏差値0.84（m/s²）を採用する。
更に、(ｄ)のシステムと同様にｙ_s軸加速度計出力を
車速パルスから得られる速度（ｖ_xb_obs）×ｚ_s軸ジャイロスコープ出力ω_zs（=R)
とし（掛け算の結果の単位はm/s²）、

とする。また、ｚ_s軸加速度計出力を‐9.8（m/s²）とし、

とする。
（3）ｘ_s軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ

で固定する。また、ｙ_s軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ

で固定する。さらに、x_s軸加速度計、ｙ_s軸加速度計、z_s軸加速度計出力に含まれるバイアス

で固定する。
（4）ロール角＝0の測定量を、後述のカルマンフィルタ補正部４０で使用する。
以上の（1）−（4）におけるパラメータの設定例は代表的なものであり、設計者が意図に応じてデザインパラメータとして変化させることも可能である。ただしオーダーが変わるほどの変化でない限り、システム性能はさほど変わらない。 (E) Vehicle speed pulse sensor, accelerometer 0 axis, and gyroscope 1 axis system:
In such a system, the sensor output is determined according to the following (1) to (4).
(1) The z _s axis is used for the gyroscope 1 axis (Fig. 3 (C)).
(2) Set the xs-axis gyroscope output to 0 (ω _xs = P = 0) as in the system (c)

And then, the y _s-axis gyroscope output and _{0 (ω ys = Q = 0} ),

And Also, the _xs axis accelerometer output is set to 0,

And That is, it is assumed that the magnitude of sensor forward acceleration due to vehicle motion is the addition of white noise with an average value of 0 (m / s ² ) and a standard deviation of 0.8 (m / s ² ). This standard deviation value is different from 0.5 to 1.0 (m / s ² ) depending on the number of slopes and the acceleration / deceleration of the driver. Here, we actually run the test course including the spiral parking lot shown in FIG. The standard deviation value 0.84 (m / s ² ) of the collected x _s- axis accelerometer output data is used.
Furthermore, as with the system in (d), the y _s- axis accelerometer output is obtained from the vehicle speed pulse (v _xb _obs) × z _s- axis gyroscope output ω _zs (= R)
(The unit of the result of multiplication is m / s ² )

And Also, set the z _s- axis accelerometer output to -9.8 (m / s ² )

And
(3) Bias and sensitivity included in _xs axis gyroscope output

Secure with. Further, each of the bias and sensitivity contained in y _s axis gyroscope output

Secure with. In addition, bias included in x _s- axis accelerometer, y _s- axis accelerometer, and z _s- axis accelerometer output

Secure with.
(4) The measured amount with the roll angle = 0 is used in the Kalman filter correction unit 40 described later.
The parameter setting examples in (1) to (4) above are typical, and the designer can change them as design parameters according to the intention. However, the system performance will not change much unless the order changes.

のように表されているのは、ホワイトノイズでモデル化された測定ノイズであり、その標準偏差は

の様に表されている。 (F) Calculation of Kalman Filter Correction Unit Next, a calculation method of the Kalman filter correction unit 40 that accompanies low-frequency measurement value input will be described. In the following measurement equation,

Is the measurement noise modeled with white noise, and its standard deviation is

It is expressed as

を用いる。但し、
ｖ_xb_obs = 車速パルス間距離をパルス間隔で割ることで得られ、ｖ_yb_obs = 0、ｖ_zb_obs = 0である。これらの標準偏差は例えば

である。ただし、これらの値はデザイン値であり、ここに記した値は目安である。 (A) Normal driving condition In the normal driving condition, the following measurement equation

Is used. However,
v _{xb —} obs = obtained by dividing the distance between the vehicle speed pulses by the pulse interval, and v _yb _obs = 0 and v _zb _obs = 0. These standard deviations are for example

It is. However, these values are design values, and the values described here are approximate.

であるとし、これに加えて以下の測定方程式を用いる。

である。 (B) Static state When the vehicle speed pulse does not enter for more than 2 seconds, it is determined that the vehicle is stationary. At rest, the measured values and their standard deviations are

In addition to this, the following measurement equation is used.

It is.

但し、測定値と測定誤差標準偏差はGPS受信機５０から得られる。 (C) GPS receiver measurement state When the following GPS receiver measurement value is obtained, the following GPS receiver measurement result is used in addition to the measurement of either the normal running state or the stationary state.

However, the measurement value and the measurement error standard deviation are obtained from the GPS receiver 50.

これは、
c₂₁ = sin(センサーロール角)・cos(センサーピッチ角)
に相当し、c₂₁ = 0を満たすことで、ロール角＝0が自動的に満たされるからである。 When information on roll angle = 0 is sent, the following measurement equation is used.

this is,
c ₂₁ = sin (sensor roll angle) ・ cos (sensor pitch angle)
C ₂₁ = This is because satisfying 0 automatically satisfies the roll angle = 0.

但し、測定ベクトル

は以下のように与えられる。

(D) Nonlinear measurement equation The above equations (21) to (24) can be put together and simply expressed as a determinant as shown by the nonlinear measurement equation of (25).

However, measurement vector

Is given by:

但し、微小擾乱測定ベクトル

は以下のように与えられる。

また、行列（観測行列という）

は図８に示すように与えられる。通常走行状態では図８の行列内の（1）の行を観測行列として使用し、静止状態では（1）の行と（2）の行を観測行列として使用する。もしもGPS受信機測定が同時に得られるときには（1）の行又は（1）の行と（2）の行に（3）の行を合わせて観測行列として使用する（特願２００７−２３３５１５参照）。さらに、センサー構成によっては（４）の行を加える。 (E) Linear measurement equation (Kalman filter observation formula)
When the equation (25) is linearized for use in the Kalman filter, the following linear measurement equation (observation equation of the Kalman filter) is obtained.

However, micro disturbance measurement vector

Is given by:

Matrix (referred to as observation matrix)

Is given as shown in FIG. In the normal running state, the row (1) in the matrix of FIG. 8 is used as the observation matrix, and in the stationary state, the rows (1) and (2) are used as the observation matrix. If GPS receiver measurements can be obtained at the same time, the (1) row or the (1) row and the (2) row are combined with the (3) row and used as an observation matrix (see Japanese Patent Application No. 2007-233515). Furthermore, the line (4) is added depending on the sensor configuration.

但し、上式において、

である。また、

は測定値に関するコバリアンス行列であり、使用する

に応じて変化する。すなわち、コバリアンス行列は通常走行状態であれば(33a)で表現され、静止状態であれば(33b)式で表現される。

GPS受信機測位が得られるときには、上記いずれかの走行状態に加え、GPS受信機の測定誤差に関するコバリアンスを以下のように付け加える。

センサー構成によっては、SCコントローラ２０から入力するロール角＝0の測定量を使用する際、以下の測定誤差に関するコバリアンスを付け加える。

When the measured value is obtained, iterative calculation of the following formulas (29)-(31) is performed n times from i = 0. In general, if n = 2 or more iterations, the accuracy is significantly improved compared to the case where no iteration is performed, and it is not necessary to do 10 or more iterations. This method of performing iterative calculation every time a positioning is obtained is a well-known method called an iterated extended Kalman filter (see the following document).
Gelb, A., Applied Optimal Estimation, MIT Press, Cambridge, MA, 1974, pp. 182-91.

However, in the above formula,

It is. Also,

Is the covariance matrix for the measured value, used

It changes according to. That is, the covariance matrix is expressed by (33a) in the normal running state, and is expressed by (33b) in the stationary state.

When GPS receiver positioning is obtained, in addition to any of the above running conditions, a covariance regarding measurement error of the GPS receiver is added as follows.

Depending on the sensor configuration, the following covariance regarding measurement error is added when using the measurement amount of roll angle = 0 input from the SC controller 20.

(G) Experimental Results FIGS. 9 to 13 show experimental results showing the effectiveness of the present invention. FIG. 14 shows, for comparison, the results of a current navigation system that does not use the present invention. The experiment method is to collect data from each sensor and GPS receiver by running on the road, then configure the navigation system of the present invention with a desktop personal computer (PC), and input the road collected sensor data to the PC for simulation. It consists of the process of doing. From the road test in various scenes, the experimental results are compared using the spiral parking lot running, which shows a large difference between systems. This is the most severe condition for estimating the azimuth angle because the signal does not reach the GPS receiver in the spiral parking lot and it rotates many times while going up and down without correction, and the performance difference of the autonomous navigation system is remarkable Because it appears in
FIGS. 9 to 14 are all plan views, and a spiral parking lot is visible where an arc is drawn. In the test run, the spiral parking lot shown in (A) of each figure is twice (course 1 and course 2), and then the spiral parking lot shown in (B) of each figure is twice (course 3 and course 4). ), Running continuously in the order of 1-2-3-4 in the figure. A continuous point trajectory is an estimated trajectory of the composite system of the present invention, and a sparse point trajectory is a trajectory of an estimated position only by a GPS receiver. In both figures, the estimated trajectory is drawn to the GPS receiver trajectory and is on the road link (straight line), but I would like you to pay attention to the azimuth at the entrance and exit of the spiral parking lot. The vehicle is running on a straight road before and after the parking lot.

  FIG. 9 shows an estimated trajectory as a result of a basic system (6-degree-of-freedom system: Acc3 Gyro3 system) composed of a sensor configuration of a vehicle speed pulse sensor, a 3-axis accelerometer, and a 3-axis gyro. It can be seen that the estimated trajectories 1 to 4 are the same as the actual travel trajectory because the approach azimuth and exit azimuth to the parking lot are in a straight line.

  FIG. 10 shows the results of a system (Acc3 Gyro2 system) comprising a vehicle speed pulse sensor, a 3-axis accelerometer, and a 2-axis gyro sensor configuration. It can be confirmed that the result is almost the same as the basic system with 6 degrees of freedom.

  FIG. 11 shows the results of a system (Acc3 Gyro1 system or Acc2 Gyro1 system) comprising a sensor configuration of a vehicle speed pulse sensor, a 3-axis or 2-axis accelerometer, and a 1-axis gyro. Here, as described above, the same algorithm is used regardless of whether the accelerometer is 3-axis or 2-axis. In this system, estimated trajectories 1, 3, and 4 in which an error of 10 degrees to 20 degrees occurs at the exit azimuth can be confirmed.

  FIG. 12 shows a result of a system including a vehicle speed pulse sensor, a single-axis accelerometer, and a single-axis gyro sensor configuration (Acc1 Gyro1 system). Similar to the system of FIG. 11, estimated trajectories 1, 3, and 4 in which an error of 10 degrees to 15 degrees occurs at the exit azimuth can be confirmed.

  FIG. 13 shows a result of a system (Acc0 Gyro1 system) including a sensor configuration of a vehicle speed pulse sensor, a 0-axis accelerometer, and a 1-axis gyro. Estimated trajectories 3 and 4 in which an error of 10 degrees to 15 degrees occurs at the exit azimuth can be confirmed. This sensor configuration system does not have pitch angle estimation performance, altitude estimation is not performed by autonomous navigation, and correction is performed only by positioning data of a GPS receiver.

  For comparison, FIG. 14 shows the result of a current navigation product that includes a vehicle speed pulse sensor, a single-axis accelerometer, and a single-axis gyro sensor configuration that does not use the present invention. This system does not take into account the effects of the roll angle and pitch angle of the vehicle, and does not use the Kalman filter process, so the sensitivity error correction and bias correction are inaccurate. It shows that the azimuth angle is large and inaccurate. That is, in the actual travel locus, the approach azimuth angle and the escape azimuth angle are in a straight line. However, in FIG. There is a significant shift.

FIGS. 15 to 19 show 2σ values (2 × standard deviation σ = 96%) regarding the estimated values of the position and the azimuth using the diagonal elements (corresponding to σ ² ) of the covariance matrix P used during the simulation calculation. The error range is plotted, with the horizontal axis representing elapsed time (sec) and the vertical axis representing error distance (m) and error angle (deg). This method is a means for discussing statistical accuracy using a single running data, and is valid when the assumption of white noise is correct. FIGS. 15 to 19 show the position error 2σ value from the entrance to the vicinity of the exit in the spiral parking lot traveling course 3 in FIGS. 9 to 13 ((A) in FIG. 15 to FIG. 19) and the azimuth error, respectively. 2σ value ((B) in FIG. 15 to FIG. 19) is shown, and is rising to the right because the GPS receiver is not corrected in the parking lot. It should be noted that when the GPS receiver is corrected outside the parking lot, the accuracy of any system converges more than the accuracy of the GPS receiver, and there is no difference between systems.

  FIG. 15 shows an accuracy result in the spiral parking lot traveling 3 of the basic system with 6 degrees of freedom. The position accuracy and azimuth angle accuracy are the highest among the five systems (2σ value is the smallest), indicating the reference accuracy in the system.

  FIG. 16 shows an accuracy result in the spiral parking lot traveling course 3 of the Acc3 Gyro2 system. Compared to the basic system with 6 degrees of freedom, the plane position accuracy (N, E) is almost the same, and although the height error is about 1.5 times larger, it can be confirmed that the azimuth accuracy is also almost the same.

  FIG. 17 shows an accuracy result in the spiral parking lot traveling course 3 of the Acc3 Gyro1 system. Compared to the basic system with 6 degrees of freedom, the plane position accuracy (N, E) is almost the same, the height error is about twice, and the azimuth angle error is about twice.

  FIG. 18 shows an accuracy result in the spiral parking lot traveling course 3 of the Acc1 Gyro1 system. Compared to the basic system with 6 degrees of freedom, the plane position accuracy (N, E) is almost the same, the height error is about twice, and the azimuth angle error is about 1.5 times. At this point, it was found that the cheaper Acc1 Gyro1 system performed better than the Acc3 Gyro1 system when using the specific example of this proposal.

  FIG. 19 shows the accuracy results in the spiral parking lot traveling course 3 of the Acc0 Gyro1 system. Compared to the basic system with 6 degrees of freedom, the plane position accuracy (N, E) is almost the same, but the height error is about 7 times, and the Acc0 Gyro1 system tracks the height direction with autonomous navigation. Because it does not. On the other hand, the azimuth angle error is about 1.5 times, which is almost the same as the Acc1 Gyro1 system.

FIG. 20 is a chart summarizing the accuracy performance. The standard cost is shown in the rightmost column. This is based on the mass production price of the current general vehicle spec sensor,
Approximate from (accelerometer 1 axis ¥ 500) × number of axes + (gyro 1 axis ¥ 700) × number of axes. The leftmost AiGj is the same as ACCi Gyroj.

  Unlike the method of the present invention, a method that does not use a stochastic optimal theory such as a Kalman filter or a particle filter cannot make a discussion in which the theoretical accuracy and the cost are associated with each other as shown in FIG.

  As described above, according to the present invention, the current sensor configuration is determined by evaluating information acquired from the sensor, and the difference between the determined sensor configuration and the basic system with six degrees of freedom is complemented. Measures are taken, and the Kalman filter process is always performed and the vehicle state quantity is estimated and calculated using the algorithm of the basic system with 6 degrees of freedom regardless of the sensor configuration. As a result, development efficiency is improved because a single software can handle multiple sensor configurations. In addition, more information is used as much as the sensor signal dynamically estimated and interpolated than the algorithm corresponding only to the individual sensor configuration, and the accuracy of the autonomous navigation calculation is improved. For this reason, even if the number of sensor axes is reduced, the decrease in accuracy is less than that of algorithms corresponding only to individual sensor configurations.

It is a basic lineblock diagram of the compound navigation system of the present invention. It is the 1st explanatory view of a sensor fixed coordinate system and a sensor composition inside a sensor board. It is the 2nd explanatory view of a sensor fixed coordinate system and a sensor composition inside a sensor board. It is explanatory drawing of a vehicle coordinate system and a NED coordinate system. It is a flowchart of the process which respond | corresponds to the multiple sensor structure based on this invention with single software. 5 is an explanatory diagram for explaining components of a matrix F. FIG. It is explanatory drawing explaining the component of the matrix Gk. FIG. 5 is an explanatory diagram illustrating components of a matrix H. It is explanatory drawing of the estimation locus | trajectory of the system (basic system of 6 degrees of freedom) which consists of a sensor structure of a vehicle speed pulse sensor, a 3-axis accelerometer, and a 3-axis gyro. It is explanatory drawing of the estimated locus | trajectory of the system (Acc3 Gyro2 system) which consists of a sensor structure of a vehicle speed pulse sensor, a 3-axis accelerometer, and a 2-axis gyro. It is explanatory drawing of the estimated locus | trajectory of the system (Acc3 Gyro1 system or Acc2 Gyro1 system) which consists of a sensor structure of a vehicle speed pulse sensor, a 3-axis or 2-axis accelerometer, and a 1-axis gyro. It is explanatory drawing of the estimation locus | trajectory of the system (Acc1 Gyro1 system) which consists of a sensor structure of a vehicle speed pulse sensor, a 1-axis accelerometer, and a 1-axis gyro. It is explanatory drawing of the estimated locus | trajectory of the system (Acc0 Gyro1 system) which consists of a sensor structure of a vehicle speed pulse sensor, a 0-axis accelerometer, and a 1-axis gyro. It is explanatory drawing of the estimated locus | trajectory of the system which consists of a sensor structure of a vehicle speed pulse sensor, 1 axis | shaft accelerometer, and 1 axis | shaft gyroscope which does not use this invention. It is explanatory drawing of 2σ theoretical value (accuracy) regarding the position and azimuth of a basic system with 6 degrees of freedom. It is explanatory drawing of 2σ theoretical value (accuracy) regarding the position and azimuth of the Acc3 Gyro2 system. It is explanatory drawing of 2σ theoretical value (accuracy) regarding the position and azimuth of the Acc3 Gyro1 system. It is explanatory drawing of 2σ theoretical value (accuracy) regarding the position and azimuth of the Acc1 Gyro1 system. It is explanatory drawing of 2σ theoretical value (accuracy) regarding the position and azimuth of the Acc0 Gyro1 system. It is the chart which summarized accuracy performance and cost for every sensor composition.

Explanation of symbols

DESCRIPTION OF SYMBOLS 10 Sensor board 20 SC controller 30 Autonomous navigation calculation part 40 Kalman filter correction part 50 GPS receiver 60 Calculation part

Claims (14)

In a navigation method of an in-vehicle navigation system that combines a plurality of sensors including a vehicle speed pulse sensor, an accelerometer, an angular velocity meter (gyroscope), and a GPS receiver,
Select a given sensor configuration system from multiple systems that consist of several sensor combinations,
Complementing the difference between the selected sensor configuration system and the basic system with all sensors,
Kalman filter processing is performed based on the control algorithm of the basic system to obtain estimated values of the position, speed, and attitude of the vehicle.
A navigation method characterized by that. The sensor configuration system is at least
GPS receiver, vehicle speed pulse sensor, accelerometer 3-axis, gyroscope 3-axis,
GPS receiver, vehicle speed pulse sensor, accelerometer 3 axis, gyroscope 2 axis,
GPS receiver, vehicle speed pulse sensor, 3 axes of accelerometer, 1 axis of gyroscope,
GPS receiver, vehicle speed pulse sensor, 2 axes of accelerometer, 1 axis of gyroscope,
GPS receiver, vehicle speed pulse sensor, accelerometer 1 axis, gyroscope 1 axis,
GPS receiver, vehicle speed pulse sensor, accelerometer not used (0 axis), gyroscope 1 axis,
The basic system is a six-degree-of-freedom system including a GPS receiver, a vehicle speed pulse sensor, three axes of accelerometers, and three axes of gyroscopes.
The navigation method according to claim 1, wherein: The selected sensor configuration system is preset at the time of system design, or is automatically determined based on a signal obtained from the sensor.
The navigation method according to claim 1 or 2, wherein Complementing means for complementing the difference is provided, and the complementing means adopts 0 or white noise as the output value of the sensor that is missing from the basic system in the selected sensor configuration system.
The navigation method according to claim 1 or 2, characterized in that Complement means for complementing the difference is provided, the complement means adopts a constant as the output value of the sensor that is missing from the basic system in the selected sensor configuration system,
The navigation method according to claim 1 or 2, characterized in that Complementing means for complementing the difference is provided, and the complementing means adopts a function value of a function having an output value of another sensor as a variable as an output value of a sensor that is missing from the basic system in the selected sensor configuration system. To
The navigation method according to claim 1 or 2, characterized in that Complement means for complementing the difference is provided, and the complement means has a roll angle to prevent the roll angle estimator from diverging when the selected sensor configuration system does not have a sensor for estimating the roll angle. Enter the measured quantity of 0 into the Kalman filter.
The navigation method according to claim 1 or 2, characterized in that In a vehicle navigation system that combines multiple sensors including a vehicle speed pulse sensor, an accelerometer, an angular velocity meter (gyroscope), and a GPS receiver.
Complementary part that complements the difference between a predetermined sensor configuration system selected from multiple systems composed of several sensor combinations and the basic system with all sensors,
Autonomous navigation calculation unit that calculates autonomous navigation using the complemented values and sensor output signal of the system,
A processing unit that performs Kalman filter processing using autonomous navigation calculation results and GPS measurement results based on the control algorithm of the basic system, and obtains estimated values of the position, speed, and posture of the vehicle,
An in-vehicle navigation system characterized by comprising: The sensor configuration system is at least
GPS receiver, vehicle speed pulse sensor, accelerometer 3-axis, gyroscope 3-axis,
GPS receiver, vehicle speed pulse sensor, accelerometer 3 axis, gyroscope 2 axis,
GPS receiver, vehicle speed pulse sensor, 3 axes of accelerometer, 1 axis of gyroscope,
GPS receiver, vehicle speed pulse sensor, 2 axes of accelerometer, 1 axis of gyroscope,
GPS receiver, vehicle speed pulse sensor, accelerometer 1 axis, gyroscope 1 axis,
GPS receiver, vehicle speed pulse sensor, accelerometer not used (0 axis), gyroscope 1 axis,
The basic system is a six-degree-of-freedom system including a GPS receiver, a vehicle speed pulse sensor, three axes of accelerometers, and three axes of gyroscopes.
The in-vehicle navigation system according to claim 8. Means for presetting the selected sensor configuration system during system design, or means for automatically determining the selected sensor configuration system based on a signal obtained from the sensor;
An in-vehicle navigation system according to claim 8 or 9, further comprising: The complement means adopts 0 or white noise as the output value of the sensor that is missing from the basic system in the selected sensor configuration system,
The in-vehicle navigation system according to claim 8 or 9, characterized in that. The complement means employs a constant as the output value of the sensor that is missing from the basic system in the selected sensor configuration system,
10. The navigation system according to claim 8 or 9, wherein: The complementing unit employs a function value of a function having an output value of another sensor as a variable as an output value of a sensor that is missing from the basic system in the selected sensor configuration system.
The in-vehicle navigation system according to claim 8 or 9, characterized in that. When the selected sensor configuration system does not have a sensor for estimating a roll angle, the complementing means inputs a measurement amount having a roll angle of 0 to a Kalman filter in order to prevent the roll angle estimation amount from diverging.
The in-vehicle navigation system according to claim 8 or 9, characterized in that.

Images