JP2009250778A

JP2009250778A - Repeated calculation control method and device in kalman filter processing

Info

Publication number: JP2009250778A
Application number: JP2008098856A
Authority: JP
Inventors: Takayuki Watanabe; 隆行渡辺; Takehiko Sakagami; 武彦阪上; Kotaro Wakamatsu; 浩太郎若松
Original assignee: Alpine Electronics Inc
Current assignee: Alpine Electronics Inc
Priority date: 2008-04-07
Filing date: 2008-04-07
Publication date: 2009-10-29
Anticipated expiration: 2028-04-07
Also published as: JP5164645B2

Abstract

<P>PROBLEM TO BE SOLVED: To provide "repeated calculation control method and device in Kalman filter processing" for accurately estimating a position, speed, and the like with a small number of repeated calculations. <P>SOLUTION: In the Kalman filter processing for estimating a state quantity including the position and speed of a vehicle and the attitude of a sensor, a Kalman filter processing section 14 calculates the state quantity including the position and speed of the vehicle and the attitude of the sensor, and a repeated calculation stop control section 22 of the Kalman filter calculates difference between the previous time and this time in a predetermined state quantity, of the calculated state quantity, or a value corresponding to the state quantity. When the difference becomes larger than the previous difference, or when the difference becomes a predetermined value or lower, the repeated calculation stop control section 22 stops the repeated calculations of the state quantity by the Kalman filter processing section 14, and a correction state quantity output section 21 outputs the state quantity calculated by the Kalman filter processing section 14 as correction state quantity. <P>COPYRIGHT: (C)2010,JPO&INPIT

Description

本発明はカルマンフィルタ処理における繰り返し演算制御方法及び装置に関わり、特に、車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理における繰り返し演算を制御する繰り返し制御方法及び繰り返し演算制御装置に関する。 The present invention relates to an iterative calculation control method and apparatus for Kalman filter processing, and more particularly to an iterative control method and apparatus for controlling an iterative calculation in Kalman filter processing for estimating a state quantity including a vehicle position, speed, and sensor attitude. .

自律航法システムは、車両の距離センサーである車速パルスセンサーや、加速度計と角速度計(ジャイロスコープ)などの慣性センサーから成り、動体である車両の位置、速度、及びセンサー姿勢の推定のために広く使用されている。 Autonomous navigation systems consist of vehicle speed sensors, which are distance sensors for vehicles, and inertial sensors such as accelerometers and angular velocity meters (gyroscopes). in use.

慣性航法システム（Inertial Navigation System：INS）は、通常３軸加速度計と３軸ジャイロスコープからなり、動体の６自由度、即ち３次元の並進運動と３次元の回転運動を同時に推定するために使用される。例えば、航空・宇宙機のナビゲーションは、かかる慣性航法システムを用いて、動体の６自由度、即ち３次元の並進運動と３次元の回転運動を同時に推定する。車両ナビゲーションにおいては車速パルスセンサーを使用することで、コストの高い慣性センサーを削減する場合が多い。 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.

自律航法は外部信号に頼らずにセンサー出力に基づいて推定位置の更新を行うことができる。しかし、センサー出力に含まれるノイズなどのエラーが時間の経過とともに積分計算に伴って蓄積するため、長時間は使用できない。そこで、衛星信号の届く範囲では有限誤差の位置推定を常時与える全地球測位システム（Global Positioning System: GPS受信機）と自律航法システムを、カルマンフィルタなどの最適確率アルゴリズムで結合することにより、両システムの性能上の問題を補間・解決する手法が考案された(特許文献１)。このようなシステムは複合ナビゲーションシステム（Integrated Navigation System）と呼ばれ、現在では車両ナビゲーションから航空宇宙ナビゲーションまで、幅広く利用されている。 Autonomous navigation can update the estimated position based on the sensor output without relying on external signals. However, since errors such as noise included in the sensor output accumulate with integration over time, they cannot be used for a long time. 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.

カルマンフィルタ処理は所定時刻における予測値と観測値との誤差を修正しながら、各時刻における最適な推定値を逐次求める方法である。カルマンフィルタ処理においては、予め、ある値を予測するための算出式を設定し、この算出式を用いて観測値Z(t)が得られる時刻ｎまで予測を繰り返す。時刻nで観測値が取得できれば、該観測値を用いて時刻ｎでの推定値について確率論的に定義された誤差を最小化させるような推定値補正計算を行う。
図１４はカルマンフィルタ処理の概要説明図である。カルマンフィルタにおいては、図１４に示すように、信号生成過程４１と観測過程５１に分けられる。図において、線形システムF(前記算出式により同定される)があり、そのシステムの状態をX(t)とするとき、観測行列Hを介してX(t)の一部が観測できる場合、カルマンフィルタはX(t)の最適な推定値を与える。ここで、ｗは信号生成過程にて発生する雑音であり、ｖは観測過程にて発生する雑音である。カルマンフィルタは、入力を観測値Z(t)としてカルマン処理を所定周期で繰り返し実行することにより、最適推定値X(t)を求める。 The Kalman filter process is a method of sequentially obtaining an optimum estimated value at each time while correcting an error between a predicted value and an observed value at a predetermined time. In the Kalman filter process, a calculation formula for predicting a certain value is set in advance, and the prediction is repeated using this calculation formula until time n at which the observed value Z (t) is obtained. If the observed value can be acquired at time n, an estimated value correction calculation is performed using the observed value so as to minimize an error defined stochastically for the estimated value at time n.
FIG. 14 is a schematic explanatory diagram of Kalman filter processing. The Kalman filter is divided into a signal generation process 41 and an observation process 51 as shown in FIG. In the figure, there is a linear system F (identified by the above formula), and when the state of the system is X (t), if a part of X (t) can be observed through the observation matrix H, the Kalman filter Gives an optimal estimate of X (t). Here, w is noise generated in the signal generation process, and v is noise generated in the observation process. The Kalman filter obtains the optimum estimated value X (t) by repeatedly executing the Kalman process at a predetermined cycle with the input as the observed value Z (t).

カルマンフィルタ処理におけるシステムモデルの状態式は次式

により表現される。システム状態変数δXは各軸速度、各軸位置、センサー姿勢等の状態量を含み後述するように

と表現できる。観測値が得られる周期より短い周期T毎に(1)式の演算が実行され、得られた状態量が出力される。 The state equation of the system model in Kalman filter processing is

It is expressed by The system state variable δX includes state quantities such as axis speed, axis position, sensor attitude, etc.

Can be expressed as The calculation of equation (1) is executed every period T shorter than the period in which the observed value is obtained, and the obtained state quantity is output.

また、本発明のカルマンフィルタの観測式は

と表現される。(1）式により得られるシステム状態変数は時間の経過と共に誤差が累積してゆく。そこで、観測値δZ(k)が得られる毎に(3)式を用いてシステム状態変数δX(k)の最適推定値を求め、以後、該最適推定値を用いて(1)式により、次の観測値が得られるまでシステム状態量δX(k)を計算して出力する。 The observation formula of the Kalman filter of the present invention is

It is expressed. Errors accumulate in the system state variables obtained by equation (1) over time. Therefore, every time the observed value δZ (k) is obtained, the optimum estimated value of the system state variable δX (k) is obtained using the equation (3), and thereafter the following equation (1) is obtained using the optimum estimated value. System state quantity δX (k) is calculated and output until the observed value is obtained.

すなわち、カルマンフィルタは、観測値Z(t)(=δZ(t))が得られる毎に（観測値Z(t)の入力周期で）、以下の(4)式を実行することにより最適推定値X(t|t)(=δX(t|t))を求める。ただし、時刻ｊまでの情報に基づく時刻iでのAの推定値をＡ(i|j)と表記するものとし、次式においてX(t｜t−1)は事前推定値、K(t)はカルマンゲインである。

(4)式の右辺{ }内の差は理想的に零であるが、実際には非零であり、該差にカルマンゲインを乗算して最適推定値X(t|t)を計算する。事前推定値およびカルマンゲインはそれぞれ

と表現でき、事前推定値X(t｜t−1)は、Z(t)の入力周期より短い周期で(1)式に相当する(5a)式により更新される。 In other words, the Kalman filter executes the following equation (4) every time the observed value Z (t) (= δZ (t)) is obtained (with the input period of the observed value Z (t)): X (t | t) (= ΔX (t | t)) is obtained. However, the estimated value of A at time i based on information up to time j is expressed as A (i | j), and X (t | t−1) is a prior estimated value and K (t) in the following equation: Is the Kalman gain.

The difference in the right side {} of equation (4) is ideally zero, but is actually non-zero. The difference is multiplied by the Kalman gain to calculate the optimum estimated value X (t | t). The prior estimate and Kalman gain are

The prior estimated value X (t | t−1) is updated by the equation (5a) corresponding to the equation (1) at a cycle shorter than the input cycle of Z (t).

また、(5b)式におけるＰは状態量Xの誤差共分散行列であり、P(t|t-1)は誤差共分散の予測値、P(t-1|t-1)は誤差共分散であり、それぞれ

である。Ｖは観測過程で発生する雑音ｖの分散、Ｗは信号過程で発生する雑音wの分散である。添字の（・）T は転置行列を意味し、(・)-1は逆行列を意味する。また、Ｉは単位行列である。さらに、ＶとＷは平均０の白色ガウス雑音であり、互いに無相関である。上記のようなカルマンフィルタにおいて、状態量Ｘと誤差共分散Pの初期値に適当な誤差を与えてやり、新しい観測が行われる度に(4)式により最適推定値を求め、以後、(5a)式により、状態量Ｘを計算することにより該状態量の精度を向上することができる。 Further, P in equation (5b) is an error covariance matrix of the state quantity X, P (t | t-1) is a predicted value of error covariance, and P (t-1 | t-1) is an error covariance. And each

It is. V is a variance of noise v generated in the observation process, and W is a variance of noise w generated in the signal process. The subscript (•) T means a transposed matrix, and (•) -1 means an inverse matrix. I is a unit matrix. Furthermore, V and W are white Gaussian noises with an average of 0 and are uncorrelated with each other. In the Kalman filter as described above, an appropriate error is given to the initial values of the state quantity X and the error covariance P, and an optimum estimated value is obtained by the equation (4) every time a new observation is made. Thereafter, (5a) By calculating the state quantity X using the equation, the accuracy of the state quantity can be improved.

ところで、(4)式により得られる状態量は1回の演算では誤差が大きい。このため、繰り返し演算を行なうことにより状態量を誤差の少ない値に収束させることが行なわれている。観測値が得られるたびに反復計算を行うこの方法は、反復拡張カルマンフィルタ（Iterated Extended Kalman Filter）と呼ばれる周知の方法である(非特許文献１)。
しかし、繰り返し演算(イタレーション)回数が多いほど、状態量の収束は早くなるが、逆に時間（処理負荷）がかかるという問題がある。このため、従来は、イタレーション処理中、状態量の前回補正量dx1と今回補正量dx0の差分（補正効果）をチェックし、補正効果が閾値（0に非常に近い値）TH以下になった場合、十分に収束したとみなし、イタレーション終了していた。 By the way, the state quantity obtained by equation (4) has a large error in one calculation. For this reason, the state quantity is converged to a value with less error by repeatedly performing the calculation. This method of performing iterative calculation each time an observation value is obtained is a well-known method called an iterated extended Kalman filter (Non-patent Document 1).
However, the larger the number of iterations (iteration), the faster the state quantity converges, but there is a problem that it takes time (processing load). For this reason, conventionally, during the iteration process, the difference (correction effect) between the previous correction amount dx1 and the current correction amount dx0 was checked, and the correction effect was below the threshold (a value very close to 0) TH. If so, it was considered that it had converged sufficiently, and the iteration was completed.

カルマンフィルタ処理により状態量を繰り返し補正すると、徐々に真値に近づくため、通常はイタレーションを実行する度に、補正効果（補正量）は0に近づいていく。しかし、実際の動きとして、補正効果が前回より大きくなったりして閾値以下にならない場合がある。図１５はセンサー(加速度計及びジャイロ)の姿勢を表わす状態量より計算されるセンサー姿勢値c10の補正量dc10とイタレーション回数との関係特性A、補正量dc10の変化量とイタレーション回数の関係特性Bであり、補正量dc10の変化量（特性Ｂ）は大きくなったり、小さくなったりして補正値変化量の閾値TH以下にならない。この現象が発生すると、いつまでたってもイタレーションが終了せず、カルマンフィルタにおける演算量が増大すると共に、補正した状態量を出力できない問題が生じる。この根本原因は不明であるが、状態量をfloat型で扱うため、数値の桁数が少なく（荒く）なり、補正量の誤差が大きくなることが考えられる。 When the state quantity is repeatedly corrected by the Kalman filter processing, the value gradually approaches the true value, so that the correction effect (correction amount) usually approaches 0 each time the iteration is executed. However, as an actual movement, there is a case where the correction effect becomes larger than the previous time and does not fall below the threshold value. FIG. 15 shows the relationship between the correction amount dc10 of the sensor attitude value c10 calculated from the state quantity representing the attitude of the sensor (accelerometer and gyroscope) and the number of iterations, and the relationship between the change amount of the correction quantity dc10 and the number of iterations. It is characteristic B, and the change amount (characteristic B) of the correction amount dc10 becomes larger or smaller and does not fall below the threshold value TH of the correction value change amount. When this phenomenon occurs, the iteration does not end indefinitely, the amount of calculation in the Kalman filter increases, and there is a problem that the corrected state quantity cannot be output. The root cause is unknown, but since the state quantity is handled in the float type, the number of digits of the numerical value is small (coarse), and the error of the correction amount may be large.

従来技術として、GPS測位位置又は自律航法測位位置のいずれか、及び、過去の自律航法測位位置の履歴に基づいて予測した予測位置を用いて移動体の位置をカルマンフィルタにより推定する技術がある（特許文献２）。また、電波航法で得られる絶対測位情報が高信頼度であれば、電波航法による絶対測位情報を利用して、自律航法による測位結果の測定誤差を推定し、電波航法による絶対測位情報が高信頼度でない場合は、捕捉衛星の中から受信パワーや仰角がしきい値よりも大きい高信頼度衛星を選択し、この高信頼度衛星との間の擬似距離及びドップラ周波数を算出しこれを利用して、自律航法による測位結果の測定誤差を推定する従来技術（特許文献３）がある。
しかし、これらの従来技術はカルマンフィルタの少ない繰り返し回数で高精度の位置、速度等の推定を可能にするものではない。
特許３４７３１１７号特開２００７−３３３６５２号公報特開２００７−２４８２７１号公報 Gelb, A., Applied Optimal Estimation, M.I.T. Press, Cambridge, MA, 1974, pp. 182-91. As a conventional technique, there is a technique for estimating a position of a moving body by a Kalman filter using a predicted position predicted based on either a GPS positioning position or an autonomous navigation positioning position and a history of past autonomous navigation positioning positions (patent) Reference 2). If the absolute positioning information obtained by radio navigation is highly reliable, the absolute positioning information by radio navigation is used to estimate the measurement error of the positioning result by autonomous navigation, and the absolute positioning information by radio navigation is highly reliable. If not, select a high-reliability satellite whose received power or elevation angle is greater than the threshold from the acquired satellites, calculate the pseudorange and Doppler frequency with this high-reliability satellite, and use this. Thus, there is a conventional technique (Patent Document 3) for estimating a measurement error of a positioning result by autonomous navigation.
However, these conventional techniques do not enable highly accurate estimation of position, speed, etc. with a small number of iterations of the Kalman filter.
Japanese Patent No. 3473117 Japanese Patent Laid-Open No. 2007-333352 JP 2007-248271 A Gelb, A., Applied Optimal Estimation, MIT Press, Cambridge, MA, 1974, pp. 182-91.

以上から、本発明の目的は、少ない繰り返し回数で高精度の位置、速度等の状態量の推定を可能にすることである。
本発明の別の目的は、補正量の変化量が閾値TH以下にならない場合であっても、繰り返し演算を停止してカルマンフィルタの演算量を減少することである。 From the above, an object of the present invention is to enable estimation of state quantities such as position and speed with high accuracy with a small number of repetitions.
Another object of the present invention is to reduce the calculation amount of the Kalman filter by stopping the repeated calculation even when the change amount of the correction amount does not become the threshold value TH or less.

本発明は車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理における繰り返し演算制御方法及び繰り返し演算制御装置である。
・繰り返し演算制御方法
本発明のカルマンフィルタ処理における繰り返し演算制御方法は、車両の位置、速度及びセンサーの姿勢を含む状態量を演算するステップ、演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回との差分を演算するステップ、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力するステップ、を有している。なお、前記状態量に応じた値は、加速度センサーの姿勢ピッチ角およびジャイロセンサの姿勢ヨー角に依存する状態量に応じた値である。
・繰り返し演算制御装置
本発明のカルマンフィルタ処理における繰り返し演算制御装置は、車両の位置、速度及びセンサーの姿勢を含む状態量を演算する状態量演算部、演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回の差分を演算する差分演算部、該差分と前回の差分の大小を比較する比較部、該差分が設定値以下になったか否かを検出する検出部、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、前記状態量の繰り返し演算を停止するよう制御すると共に、演算された状態量を出力するよう制御する制御部を備えている。 The present invention is an iterative calculation control method and an iterative calculation control device in a Kalman filter process for estimating a state quantity including a vehicle position, speed, and sensor attitude.
-Iterative calculation control method The iterative calculation control method in the Kalman filter processing of the present invention includes a step of calculating a state quantity including a vehicle position, speed, and sensor attitude, a predetermined state quantity among the computed state quantities, or the state quantity A step of calculating the difference between the previous time and the current value for the value according to the above, when the difference becomes larger than the previous difference, or when the difference becomes less than or equal to the set value, stop the repeated calculation of the state quantity, A step of outputting the calculated state quantity. The value according to the state quantity is a value according to the state quantity depending on the attitude pitch angle of the acceleration sensor and the attitude yaw angle of the gyro sensor.
-Repetitive Arithmetic Control Device The repetitive arithmetic control device in the Kalman filter processing of the present invention includes a state quantity computing unit that computes a state quantity including the vehicle position, speed, and sensor attitude, and a predetermined state quantity or A difference calculation unit that calculates a difference between the previous time and the current value for a value corresponding to the state quantity, a comparison unit that compares the difference with the previous difference, and a detection unit that detects whether the difference is equal to or less than a set value. When the difference becomes larger than the previous difference, or when the difference falls below a set value, control is performed to stop the repeated calculation of the state quantity, and control is performed to output the calculated state quantity. A control unit is provided.

以上から本発明によれば、車両の位置、速度及びセンサーの姿勢を含む状態量を演算し、演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回との差分を演算し、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力するように構成したから、補正効果が前回より大きくなったり、小さくなったりして閾値以下にならない場合でも、少ない繰り返し回数で高精度の位置、速度等の推定が可能になる。又、本発明によれば、補正量の変化量が閾値TH以下にならない場合であっても、繰り返し演算を停止するため、カルマンフィルタの演算量を減少することが出来、しかも状態量を出力することができる。 As described above, according to the present invention, the state quantity including the vehicle position, speed, and sensor attitude is calculated, and the predetermined state quantity or the value corresponding to the state quantity among the calculated state quantities is calculated between the previous time and the current time. The difference is calculated, and when the difference becomes larger than the previous difference, or when the difference falls below a set value, the state quantity repeated calculation is stopped and the calculated state quantity is output. Therefore, even when the correction effect becomes larger or smaller than the previous time and does not fall below the threshold, it is possible to estimate the position, speed, etc. with high accuracy with a small number of iterations. In addition, according to the present invention, even when the amount of change in the correction amount does not fall below the threshold value TH, the computation is stopped repeatedly, so the computation amount of the Kalman filter can be reduced and the state quantity can be output. Can do.

（A）車載ナビゲーション装置の位置検出部の構成
図１は本発明を適用できる車載ナビゲーション装置の位置検出部の構成図である。自律系の入力信号としては、センサーボード１０上の慣性センサーから得られる信号と、別のケーブルを通じて車体から取り込まれる車速パルスがある。慣性センサーとしては加速度計(加速度３軸)１０a、ジャイロスコープ（角速度３軸)１０bを採用し、車速パルスを発生するセンサーとしては車両が所定距離移動する毎に１個のパルスを発生する車速センサー１１を採用する。図２(Ａ)はセンサーボード１０に固定したセンサー座標系Xs-Ys-Zsを示している。なお、以下の数式の中でセンサー（ボード）座標系に関する表現であることを、添え字の”ｓ”によって表現することに注意すべきである。図２(B)は、センサーボード１０において、加速度計１０ａの3軸（Acc x, Acc y, Acc z）およびジャイロスコープ１０ｂの3軸（Gyro x, Gyro y, Gyro z）から成る、即ち6自由度の基本システムのセンサー構成を示したものである。３軸加速度計Acc x〜Acc zは、センサー座標系の三つの座標方向（ｘ，ｙ，ｚ）における加速度を検出し、また３軸ジャイロスコープGyro x〜Gyro zは、センサー座標系の三つの座標軸（ｘ，ｙ，ｚ）周りの角速度P,Q,Rを検出する。
なお、図３（A）は、車両に固定した座標系Xb-Yb-Zbを示しており、以下の数式の中で車両固定座標系に関する表現であることを、添え字の”b”によって表現する。又、図３(B)は、ある緯度、経度における、地表固定座標系North-East-Down（NED座標系）を示しおり、以下の数式の中でNED座標系に関する表現であることを、添え字の”n”によって表現する。 (A) Configuration of Position Detection Unit of In-vehicle Navigation Device FIG. 1 is a configuration diagram of a position detection unit of an in-vehicle navigation device to which the present invention can be applied. As an autonomous input signal, there are a signal obtained from an inertial sensor on the sensor board 10 and a vehicle speed pulse taken from the vehicle body through another cable. As an inertial sensor, an accelerometer (acceleration 3-axis) 10a and a gyroscope (angular velocity 3-axis) 10b are used. As a sensor for generating a vehicle speed pulse, a vehicle speed sensor that generates one pulse every time the vehicle moves a predetermined distance. 11 is adopted. FIG. 2A shows a sensor coordinate system Xs-Ys-Zs fixed to the sensor board 10. 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 is composed of three axes (Acc x, Acc y, Acc z) of the accelerometer 10a and three axes (Gyro x, Gyro y, Gyro z) of the gyroscope 10b. It shows the sensor configuration of the basic system of freedom. The three-axis accelerometers Acc x to Acc z detect accelerations 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. 3 (A) shows a coordinate system Xb-Yb-Zb fixed to the vehicle, and the expression relating to the vehicle fixed coordinate system is expressed by the subscript “b” in the following equation. To do. Fig. 3 (B) shows the ground fixed coordinate system North-East-Down (NED coordinate system) at a certain latitude and longitude, and the expression below is related to the NED coordinate system. Expressed by the letter “n”.

自律航法計算部１２は６自由度の基本システムのアルゴリズムを用いて、車両の現在の位置、速度及び姿勢を含む車両状態量を、後述する計算式に従って高周波数で、例えば２５Hzで更新する。
GPS受信機１３は、複数の人工衛星から距離と距離変化率に関する信号を受信することで、車両のアンテナの位置(緯度、経度、高さ)と車両速度(北方向速度、東方向速度、上下方向速度)の測定値を１Hzで提供する。
カルマンフィルタ演算部１４は車両の現在の位置、速度及び姿勢を含む車両状態量の補正を行う。例えば、カルマンフィルタ演算部１４は１０Hzの速度で、車速センサーから得られる速度(走行時)あるいはジャイロから出力される角速度オフセット(静止時)を観測値として用いて状態量補正処理を行って得られた補正状態量を自律航法計算部１２に入力する。又、カルマンフィルタ演算部１４は１Hzの速度でGPS受信機１３から得られる位置・速度を観測値として用いて状態量補正処理を行って得られた補正状態量を自律航法計算部１２に入力する。 The autonomous navigation calculation unit 12 updates the vehicle state quantity including the current position, speed, and attitude of the vehicle at a high frequency, for example, 25 Hz, according to a calculation formula described later, using an algorithm of a basic system with six degrees of freedom.
The GPS receiver 13 receives signals related to distance and distance change rate from a plurality of artificial satellites, so that the position (latitude, longitude, height) of the antenna of the vehicle and the vehicle speed (north speed, east speed, up and down) A measurement of direction velocity is provided at 1 Hz.
The Kalman filter calculation unit 14 corrects the vehicle state quantity including the current position, speed, and posture of the vehicle. For example, the Kalman filter calculation unit 14 is obtained by performing a state quantity correction process at a speed of 10 Hz, using a speed obtained from a vehicle speed sensor (during traveling) or an angular speed offset (during stationary) output from a gyro as an observed value. The corrected state quantity is input to the autonomous navigation calculation unit 12. Further, the Kalman filter calculation unit 14 inputs the corrected state quantity obtained by performing the state quantity correction process using the position / velocity obtained from the GPS receiver 13 at the speed of 1 Hz as an observation value, to the autonomous navigation calculation unit 12.

（B）位置検出部の処理
図４は図１の位置検出部の全体の処理フローである。
はじめに、自律航法計算部１２に３次元車両位置N、E、D、車両速度Vsp、ピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2、ジャイロのオフセットωOF、加速度センサーのオフセットαOFの初期値を設定する（ステップ１０１）。ジャイロ１０aおよび加速度センサー１０bは、側面から見たとき、車両方向と平行して車両に取り付けられるのが理想であるが、図５（Ａ）に示すように取り付け誤差があり、センサー方向は車両方向に角度A（取り付けピッチ角）を成して取り付けられる。なお、水平方向とセンサー方向の角度θをピッチ角といい、ピッチ角は傾斜角と取り付けピッチ角の和である。また、ジャイロ１０aおよび加速度センサー１０bは、平面に投影したとき、車両方向と一致して車両に取り付けられるのが理想であるが、取り付け誤差があり、図５（Ｂ）に示すように、センサー方向は車両方向に角度A2（取り付けヨー角）を成して取り付けられる。なお、北方向とセンサー方向の角度Ｙをヨー角といい、ヨー角Ｙは車両方向と取り付けヨー角の和である。 (B) Processing of Position Detection Unit FIG. 4 is an overall processing flow of the position detection unit of FIG.
First, the autonomous navigation calculation unit 12 includes the three-dimensional vehicle position N, E, D, vehicle speed Vsp, pitch angle θ, vehicle mounting pitch angle A, yaw angle Y, vehicle mounting yaw angle A2, gyro offset ωOF, acceleration sensor An initial value of the offset αOF is set (step 101). The gyro 10a and the acceleration sensor 10b are ideally attached to the vehicle in parallel with the vehicle direction when viewed from the side, but there is an attachment error as shown in FIG. 5A, and the sensor direction is the vehicle direction. Is attached at an angle A (mounting pitch angle). The angle θ between the horizontal direction and the sensor direction is referred to as a pitch angle, and the pitch angle is the sum of an inclination angle and a mounting pitch angle. Further, the gyro 10a and the acceleration sensor 10b are ideally attached to the vehicle in accordance with the vehicle direction when projected onto a plane, but there is an attachment error. As shown in FIG. Is mounted at an angle A2 (mounting yaw angle) in the vehicle direction. The angle Y between the north direction and the sensor direction is referred to as a yaw angle, and the yaw angle Y is the sum of the vehicle direction and the mounting yaw angle.

以後、自律航法計算部１２はジャイロ１０a、加速度センサー１０b、車速センサー１１の出力を取り込み（ステップ１０２）、第１周期（２５Hz周期）で後述する計算式により(２)式に示す各軸速度、各軸位置、センサー姿勢等の状態量を計算して出力する(ステップ１０３)。ついで、第２周期(１０Hz周期)になったかチェックし(ステップ１０４)、第２周期になっていなければ、ステップ１０２以降の処理を繰り返す。
第２周期になっていれば、車両速度Vxが零である状態が2秒以上続いていたかどうかによって停車判定を行う（ステップ１０５）。
停車中でなければ、第３周期(1Hz周期=GPS測位周期)になっているかチェックし（ステップ１０６）、第３周期でなければカルマンフィルタ演算部１４は、車速センサー１１の出力より計算した車両速度(観測値)Vxと自律航法計算部１２が計算した車両速度を用いてカルマンフィルタ処理により車両状態量を補正し、補正状態量を自律航法計算部１２に入力する（ステップ１０７）。このステップ１０７では、後述するカルマンフィルタの観測行列H1を用いた第１補正処理が行われる。 Thereafter, the autonomous navigation calculation unit 12 takes in the outputs of the gyro 10a, the acceleration sensor 10b, and the vehicle speed sensor 11 (step 102), and calculates the speeds of the respective axes shown in the expression (2) according to the calculation expression described later in the first period (25 Hz period). State quantities such as each axis position and sensor posture are calculated and output (step 103). Next, it is checked whether the second cycle (10 Hz cycle) has been reached (step 104). If the second cycle has not been reached, the processing from step 102 onward is repeated.
If it is in the second period, the stoppage determination is performed based on whether or not the state where the vehicle speed Vx is zero continues for 2 seconds or more (step 105).
If the vehicle is not stopped, it is checked whether it is in the third cycle (1 Hz cycle = GPS positioning cycle) (step 106). If it is not in the third cycle, the Kalman filter calculation unit 14 calculates the vehicle speed calculated from the output of the vehicle speed sensor 11. (Observation Value) The vehicle state quantity is corrected by Kalman filter processing using Vx and the vehicle speed calculated by the autonomous navigation calculation section 12, and the corrected state quantity is input to the autonomous navigation calculation section 12 (step 107). In step 107, a first correction process using a Kalman filter observation matrix H1 described later is performed.

ステップ１０６において、第３周期であればカルマンフィルタ演算部１４はGPSレシーバ１４が出力する３次元の車両位置と３次元の車両速度を観測値として用いて車両状態量を補正し、補正状態量を自律航法計算部１２に入力する(ステップ１０８)。このステップ１０８では後述するカルマンフィルタの観測行列H2を用いた第２補正処理が行われる。
ステップ１０５において、停車中であれば、第３周期(１Hz周期=GPS測位周期)になっているかチェックし（ステップ１０９）、第３周期でなければカルマンフィルタ演算部１４は前記ステップ１０７の補正処理を行うと共に、ジャイロの角速度出力信号(観測値)と自律航法計算部１２で計算した角速度信号オフセットにより車両状態量を補正し、補正状態量を自律航法計算部１２に入力する（ステップ１１０）。このステップ１１０では後述するカルマンフィルタの観測行列H3を用いた第３補正処理が行われる。
ステップ１０９において、第３周期であれば、カルマンフィルタ演算部１４は前記ステップ１０８の補正処理を行うと共に、ジャイロの角速度出力信号と自律航法計算部１２で計算した角速度信号オフセットとの差に基づいて角速度オフセット補正を行う（ステップ１１１）。このステップ１１１では後述するカルマンフィルタの観測行列H4を用いた第４補正処理が行われる。（ステップ１１１）
以上の位置検出処理によれば、GPSによる推定誤差補正より早い周波数で、誤差累積の補正を行うため精度の高い位置検出ができる。 In step 106, if it is the third period, the Kalman filter calculation unit 14 corrects the vehicle state quantity using the three-dimensional vehicle position and the three-dimensional vehicle speed output from the GPS receiver 14 as observation values, and autonomously sets the corrected state quantity. It inputs into the navigation calculation part 12 (step 108). In this step 108, a second correction process using a Kalman filter observation matrix H2 described later is performed.
In step 105, if the vehicle is stopped, it is checked whether the third period (1 Hz period = GPS positioning period) is reached (step 109). If it is not the third period, the Kalman filter calculation unit 14 performs the correction process in step 107. At the same time, the vehicle state quantity is corrected by the angular velocity output signal (observed value) of the gyro and the angular velocity signal offset calculated by the autonomous navigation calculation section 12, and the corrected state quantity is input to the autonomous navigation calculation section 12 (step 110). In step 110, a third correction process using an observation matrix H3 of a Kalman filter, which will be described later, is performed.
In step 109, if it is the third period, the Kalman filter calculation unit 14 performs the correction process of step 108, and based on the difference between the angular velocity output signal of the gyro and the angular velocity signal offset calculated by the autonomous navigation calculation unit 12, Offset correction is performed (step 111). In step 111, a fourth correction process using an observation matrix H4 of a Kalman filter, which will be described later, is performed. (Step 111)
According to the above position detection processing, the error accumulation correction is performed at a frequency faster than that of the estimation error correction by GPS, so that a highly accurate position detection can be performed.

（Ｃ）本発明の繰り返し演算制御装置
図６は本発明のカルマンフィルタ処理における繰り返し演算制御の構成図であり、カルマンフィルタ演算部１４は観測値が得られる毎に状態量（車両の位置、速度及びセンサーの姿勢を含む）を繰り返し演算し、補正状態量出力部２１はカルマンフィルタ繰り返し演算停止制御部２２からの指示により、カルマンフィルタ演算部１４が出力する補正状態量を自律航法計算部１２に出力する。カルマンフィルタ繰り返し演算停止制御部２２は、
(1)カルマンフィルタ演算部１４で演算された状態量のうち所定の状態量、あるいは該状態量に応じた値(例えばセンサー姿勢値)について前回と今回との差分を演算し、該差分が設定値以下になったとき、あるいは、
(2)該差分が前回の差分より大きくなったとき、あるいは
(3)繰り返し演算回数が設定最大回数に達したとき、
繰り返し演算停止信号CSPを発生してカルマンフィルタ演算部１４による状態量の繰り返し演算を停止する。すなわち、カルマンフィルタ繰り返し演算停止制御部２２は、上記(1)，(2)、(3)により状態量が収束したことを検出したとき、カルマンフィルタ演算部１４による状態量の繰り返し演算を停止制御すると共に、補正状態量出力部２１を制御して補正状態量を自律航法計算部１２に出力させる。
なお、前記状態量に応じた値は、例えばセンサー姿勢値ｃ10であり、加速度センサーの姿勢ピッチ角およびジャイロセンサの姿勢ヨー角に基づいて計算される。 (C) Iterative Arithmetic Control Device of the Present Invention FIG. 6 is a block diagram of the iterative arithmetic control in the Kalman filter processing of the present invention. The Kalman filter arithmetic unit 14 obtains a state quantity (vehicle position, speed and sensor every time an observed value is obtained). The correction state quantity output unit 21 outputs the correction state quantity output by the Kalman filter calculation unit 14 to the autonomous navigation calculation unit 12 in response to an instruction from the Kalman filter repetition calculation stop control unit 22. The Kalman filter iterative calculation stop control unit 22
(1) A difference between the previous time and this time is calculated for a predetermined state quantity or a value corresponding to the state quantity (for example, a sensor attitude value) among the state quantities calculated by the Kalman filter calculation unit 14, and the difference is a set value. Or when
(2) When the difference is greater than the previous difference, or
(3) When the number of repeated operations reaches the set maximum number of times,
The repeated calculation stop signal CSP is generated to stop the repeated calculation of the state quantity by the Kalman filter calculation unit 14. That is, when the Kalman filter repetitive calculation stop control unit 22 detects that the state quantity has converged by the above (1), (2), and (3), the Kalman filter repetitive calculation stop control section 22 controls to stop the repetitive calculation of the state quantity by the Kalman filter calculation unit 14. Then, the correction state quantity output unit 21 is controlled to cause the autonomous navigation calculation unit 12 to output the correction state quantity.
The value corresponding to the state quantity is, for example, the sensor attitude value c10, and is calculated based on the attitude pitch angle of the acceleration sensor and the attitude yaw angle of the gyro sensor.

(Ｄ) カルマンフィルタ処理部
図７はカルマンフィルタ処理部１４の構成図であり、カルマンゲイン計算部１４aは観測値が発生したとき、後述する状態量コバリアンスＰ、測定値コバリアンスＲ、観測行列Ｈ、ｋ番目の状態量xkを用いてｋ番目の状態量に応じたカルマンゲインＫk,i（ただし、iは繰り返し回数で、i=0,1,2….）を次式

により計算する。上式において、

である。カルマンゲインが求まれば、状態量補正部１４bは、ｋ番目の状態量を次式

により計算し、状態量コバリアンス補正部１４cは状態量コバリアンスを次式

により補正してカルマンゲイン計算部１４aに入力する。測定値コバリアンス発生部１４dは測定値に関するコバリアンス行列Ｒkを発生し、観測行列発生部１４eは所定の観測行列Hkを発生する。
図７のカルマンフィルタ処理部１４は、カルマンフィルタ繰り返し演算停止制御部２２(図６)から繰り返し演算停止信号CSPが発生するまで、iを歩進しながら上記の(7)~(10)式の演算を繰り返し演算する。 (D) Kalman Filter Processing Unit FIG. 7 is a block diagram of the Kalman filter processing unit 14, and when an observed value is generated, the Kalman gain calculating unit 14a is a state quantity covariance P, a measured value covariance R, an observation matrix H, an k th Kalman gain Kk, i (where i is the number of iterations and i = 0,1,2,...) Corresponding to the kth state quantity using the following state quantity xk

Calculate according to In the above formula,

It is. When the Kalman gain is obtained, the state quantity correction unit 14b calculates the k-th state quantity by the following equation.

The state quantity covariance correction unit 14c calculates the state quantity covariance by the following equation:

Is corrected and input to the Kalman gain calculator 14a. The measurement value covariance generation unit 14d generates a covariance matrix Rk related to the measurement values, and the observation matrix generation unit 14e generates a predetermined observation matrix Hk.
The Kalman filter processing unit 14 in FIG. 7 performs the calculations of the above equations (7) to (10) while incrementing i until the repeated calculation stop signal CSP is generated from the Kalman filter repeated calculation stop control unit 22 (FIG. 6). Calculate repeatedly.

（Ｅ）繰り返し演算停止制御
図８は図６のカルマンフィルタ繰り返し演算停止制御部の構成図である。ｃ10計算部２１aは、カルマンフィルタ処理部１４より状態量が計算されて出力される毎に、次式

によりセンサー姿勢値ｃ10を計算する。センサー姿勢ピッチ角は水平面に対するセンサーピッチ角(図５(Ａ）参照)、センサー姿勢ヨー角は北を基準方向としたときのセンサーヨー角(図５（Ｂ）)であり、それぞれ（２）式の状態量を構成する1つの要素である。状態量が収束しているか否かに対してセンサー姿勢値ｃ10が敏感であるため、このセンサー姿勢値ｃ10を状態量が収束しているか否かを判断するために使用する。演算部２１bにおいて、保持部３１は前回のセンサー姿勢値ｃ10を保存し、差演算部３２は前回と今回のセンサー姿勢値ｃ10の差dcを計算し、保存部３３は前回の差分dcを保存し、差演算部３４は今回の差分と前回の差分の差を演算すると共に、その大小を比較し、比較結果を繰り返し演算終了決定部２１cに入力する。繰り返し演算終了決定部２１cは、今回の差分が前回の差分より大きくなったとき、図１５の特性Ａ，Ｂで示すように差分は収束したものとみなして、繰り返し演算停止信号CSPを出力する。これにより、カルマンフィルタ処理部１４はカルマンフィルタ演算処理を停止し、また、補正状態量出力部２１は最後にカルマンフィルタ処理部１４から入力されている補正状態量を自律航法計算部１２に入力する。 (E) Repetitive Calculation Stop Control FIG. 8 is a block diagram of the Kalman filter repeated calculation stop control unit of FIG. Each time the c10 calculation unit 21a calculates and outputs a state quantity from the Kalman filter processing unit 14,

To calculate the sensor attitude value c10. The sensor attitude pitch angle is the sensor pitch angle with respect to the horizontal plane (see FIG. 5A), and the sensor attitude yaw angle is the sensor yaw angle (FIG. 5B) when north is the reference direction. Is one element that constitutes the state quantity. Since the sensor attitude value c10 is sensitive to whether or not the state quantity has converged, the sensor attitude value c10 is used to determine whether or not the state quantity has converged. In the calculation unit 21b, the holding unit 31 stores the previous sensor posture value c10, the difference calculation unit 32 calculates the difference dc between the previous and current sensor posture values c10, and the storage unit 33 stores the previous difference dc. The difference calculation unit 34 calculates the difference between the current difference and the previous difference, compares the difference, and repeatedly inputs the comparison result to the calculation end determination unit 21c. When the current difference becomes larger than the previous difference, the iterative calculation end determination unit 21c regards the difference as converged as shown by the characteristics A and B in FIG. 15, and outputs a repeated calculation stop signal CSP. As a result, the Kalman filter processing unit 14 stops the Kalman filter calculation process, and the correction state quantity output unit 21 inputs the correction state quantity finally input from the Kalman filter processing unit 14 to the autonomous navigation calculation unit 12.

以上と並行して、目標値到達検出部２１dは演算部３４により演算された今回の差分と前回の差分の差ddcが予め設定されている変化量閾値(図１５参照)TH以下になったか監視しており、繰り返し演算終了決定部２１cに監視結果を入力する。繰り返し演算終了決定部２１cは、差ddcが変化量閾値以下になれば（ddc<TH）、状態量は収束しているから繰り返し演算停止信号CSPを出力する。これにより、カルマンフィルタ処理部１４はカルマンフィルタ演算処理を停止し、また、補正状態量出力部２１は最後にカルマンフィルタ処理部１４から入力されている補正状態量を自律航法計算部１２に入力する。
また、最大繰り返し検出部２１eは繰り返し演算回数を監視しており、該繰り返し演算回数nを繰り返し演算終了決定部２１cに入力する。繰り返し演算終了決定部２１cは、繰り返し演算回数が設定最大演算回数（例えば２０回）に到達すれば、状態量は収束したものとみなして、繰り返し演算停止信号CSPを出力する。これにより、カルマンフィルタ処理部１４はカルマンフィルタ演算処理を停止し、また、補正状態量出力部２１は最後にカルマンフィルタ処理部１４から入力されている補正状態量を自律航法計算部１２に入力する。 In parallel with the above, the target value arrival detection unit 21d monitors whether the difference ddc between the current difference calculated by the calculation unit 34 and the previous difference is equal to or less than a preset change amount threshold (see FIG. 15) TH. The monitoring result is input to the repeated calculation end determination unit 21c. When the difference ddc is equal to or less than the change amount threshold (ddc <TH), the repeated calculation end determination unit 21c outputs the repeated calculation stop signal CSP because the state quantity has converged. As a result, the Kalman filter processing unit 14 stops the Kalman filter calculation process, and the correction state quantity output unit 21 inputs the correction state quantity finally input from the Kalman filter processing unit 14 to the autonomous navigation calculation unit 12.
In addition, the maximum iteration detection unit 21e monitors the number of iterations, and inputs the iteration number n to the iteration end determination unit 21c. When the number of repeated calculations reaches the set maximum number of calculations (for example, 20), the repeated calculation end determination unit 21c regards the state quantity as converged and outputs a repeated calculation stop signal CSP. As a result, the Kalman filter processing unit 14 stops the Kalman filter calculation process, and the correction state quantity output unit 21 inputs the correction state quantity finally input from the Kalman filter processing unit 14 to the autonomous navigation calculation unit 12.

（Ｆ）本発明の効果
図９は状態量が収束したとみなして繰り返し演算を終了したときの本発明と従来技術の繰り返し演算回数の頻度分布説明図であり、カルマンフィルタ処理を５０００回行なった場合である。従来の平均繰り返し回数は8.7回であったのが、本発明によれば平均繰り返し回数は4.8回とほぼ半減している。特に、従来技術では最大繰り返し回数まで繰り返し演算する頻度が非常に高いが、本発明によれば、最大繰り返し回数まで繰り返し演算する頻度はゼロ回である。この結果、本発明によれば時間のかかるカルマンフィルタ処理に要する演算回数を低減できる利点がある。
図１０は走行軌跡説明図で、(A)は従来技術の走行軌跡説明図、（B）は本発明の走行軌跡説明図であり、共に車両がENTより進入し、２つの螺旋駐車場を通ってEXTから脱出した走行軌跡を示している。なお、白丸が繋がって見える軌跡がカルマンフィルタ演算位置、不連続な丸がGPS３次元測位位置である。螺旋駐車場の外では走行軌跡とGPS３次元測位位置はほぼ一致している。又、本発明と従来技術の走行軌跡の精度はほぼ同じであり、本発明のようにカルマンフィルタ演算の処理負荷を削減しても精度劣化が生じない。
以上より、本発明によれば、カルマンフィルタの特性を利用し、計算結果（走行軌跡精度）をほとんど変えることなく、処理負荷を約50％以上削減できる。
以上では、自律航法計算部１２における自律航法計算アルゴリズムやカルマンフィルタ演算部１４における計算アルゴリズムの詳細説明をしなかったが、以下にその詳細を説明する。 (F) Effect of the Present Invention FIG. 9 is an explanatory diagram of the frequency distribution of the number of repeated computations according to the present invention and the prior art when iterative computation is terminated assuming that the state quantity has converged, and when Kalman filter processing is performed 5000 times. It is. Although the conventional average number of repetitions was 8.7, according to the present invention, the average number of repetitions is halved to 4.8. In particular, in the prior art, the frequency of repeated calculation up to the maximum number of repetitions is very high, but according to the present invention, the frequency of repeated calculation up to the maximum number of repetitions is zero. As a result, according to the present invention, there is an advantage that the number of operations required for the time-consuming Kalman filter processing can be reduced.
FIG. 10 is a traveling locus explanatory diagram, (A) is a traveling locus explanatory diagram of the prior art, and (B) is a traveling locus explanatory diagram of the present invention. Both vehicles enter from ENT and pass through two spiral parking lots. It shows the running track that escaped from EXT. Note that the locus where white circles are connected is the Kalman filter calculation position, and the discontinuous circle is the GPS three-dimensional positioning position. Outside the spiral parking lot, the driving trajectory and the GPS 3D positioning position are almost the same. Further, the accuracy of the traveling locus of the present invention and the prior art is almost the same, and even if the processing load of the Kalman filter calculation is reduced as in the present invention, the accuracy does not deteriorate.
As described above, according to the present invention, the processing load can be reduced by about 50% or more by using the characteristics of the Kalman filter and hardly changing the calculation result (running track accuracy).
Although the autonomous navigation calculation algorithm in the autonomous navigation calculation unit 12 and the calculation algorithm in the Kalman filter calculation unit 14 have not been described in detail above, the details will be described below.

（Ｇ）自律航法計算部における自律航法計算アルゴリズム
まず、基本となる６自由度システムの、高周波自律航法計算部３０のアルゴリズムについて説明する。なお、以降の式においては、

で示すように変数の上にドットがついているものは、その変数ｖsの時間変化率を表しているものとする。 (G) 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,

As shown by the above, a variable having a dot on it represents a time change rate of the variable vs.

（ａ）速度方程式
速度方程式は次式

で表現される。但し、

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

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

It is expressed by However,

(13a) is the velocity vector expressed in the sensor fixed coordinate system, (13b) is the gyroscope output vector expressed in the sensor fixed coordinate system, and (13c) is the accelerometer output expressed in the sensor fixed coordinate system. Vector, (13d) is a gravity vector expressed in the 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座標系への変換行列である。
（15）式は、慣性航法システム（Inertial Navigation System: INS）の一般的な手法である以下の姿勢方程式

から、車両ナビゲーション適用の際に独立な4つのパラメータだけを抜き出したものである。(16)式はより一般的な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 (15) is the following attitude equation that is a general method of the inertial navigation system (INS).

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

（ｃ）位置方程式
位置方程式は次式

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

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

Can be expressed by However, in equation (17)

(18a) is a position vector, and (18b) is a vehicle movement distance vector. In equation (18b), Np is the number of pulses per sample time, and L is the distance between pulses.

（ｄ）固定パラメータ方程式
固定パラメータ方程式は次式

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

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

It is expressed by However,

(20a) is the bias (Volt) included in the sensor coordinate system gyroscope output, (20b) is the bias (m / s2) included in the sensor coordinate system accelerometer output, and (20c) is the sensor coordinate system gyroscope. Sensitivity (rad / sec / Volt) related to output, (20d) is a transformation matrix from the sensor coordinate system to the vehicle coordinate system. In addition, p00, p10, and p20 in the equation (19) are obtained by extracting only three independent parameters from the elements of the matrix related to the mounting angle.

（ｅ）非線形状態方程式
上記（12）、(15)、(17)、（19）式をまとめて、簡単に（21）式の様に行列式で表すことができる

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

(E) Nonlinear equation of state The above equations (12), (15), (17), (19) can be put together and expressed simply as a determinant like equation (21)

Where x is a nonlinear state quantity vector given below.

（ｆ）状態量更新
CPU上で(21)式を数値積分するには、例えば、次式の状態量更新式

を用いて行えばよい。精度要求の高い場合は、周知の数値積分方法であるルンゲ・クッタ4次式(下記文献参照)などを用いてもよい。
Kreyszig, E., Advanced Engineering Mathematics, John Wiley & Sons, 1999, New York, NY.
但し、(22)式において、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
To numerically integrate equation (21) 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 (22), 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.

（ｇ）微小擾乱方程式
さて、GPS受信機などの測定値をもとに、カルマンフィルタ補正部４０がカルマンフィルタ処理で状態量ｘを補正を行うためには、自律航法計算部３０が状態量を非線形状態方程式により更新を行い、かつ、誤差量が線形方程式によって増加すると仮定して該誤差量（微小擾乱量）のコバリアンス値も更新する必要がある。そのために、（22）式の線形化を行い、微小擾乱方程式である行列式（23）を用意する。

但し、

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

ただし、(24b)は

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

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

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

また、

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

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

上式より
δc10＝−δγ・c00＋δα・c20
が得られ、この式により図８のc10計算部２１aは状態量δγ、δαを用いてセンサー姿勢値を計算することが出来る。
また、

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

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

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

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

(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 nonlinear 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 (22) is linearized to prepare a determinant (23) which is a small disturbance equation.

However,

Is the small disturbance vector given below.

However, (24b) is

Current estimate, (24c) 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).

From the above equation δc10 = −δγ ・ c00 + δα ・ c20
From this equation, the c10 calculation unit 21a in FIG. 8 can calculate the sensor attitude value using the state quantities δγ and δα.
Also,

The amount of minute disturbance

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. (23)

Are obtained as shown in FIGS. 11 and 12, respectively. However, in FIG.

（ｈ）コバリアンスの更新
以上のように得られた行列

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

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

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

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

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

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

(H) Update of covariance matrix obtained as above

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 expressions (22) and (29) are arithmetic expressions executed by the autonomous navigation calculation unit 12 at a high frequency (for example, at 25 Hz) in the basic system with 6 degrees of freedom. Equations (22) and (29) are shown below again.

（Ｈ）カルマンフィルタ演算部の計算
次に、低周波の測定値入力に伴う、カルマンフィルタ補正部４０の計算方法について説明する。なお、以下の測定方程式で、

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

の様に表されている。 (H) Calculation of Kalman Filter Calculation Unit Next, a calculation method of the Kalman filter correction unit 40 that accompanies a 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,
vxb_obs = obtained by dividing the vehicle speed pulse distance by the pulse interval, and vyb_obs = 0 and vzb_obs = 0. These standard deviations are for example

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

（ｂ）静止状態
車速パルスが2秒以上入ってこないとき、車両が静止状態にあると判定する。静止状態において、測定値及びそれらの標準偏差は

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

である。 (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受信機測定状態
以下のGPS受信機測定値が得られるときには、上記通常走行状態と静止状態のいずれかの測定に加え、以下のGPS受信機測定結果を利用する。

但し、測定値と測定誤差標準偏差は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.

ロール角＝0の情報が送られてきたときには、次の測定方程式を使用する。

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

this is,
c21 = sin (sensor roll angle) ・ cos (sensor pitch angle)
This is because roll angle = 0 is automatically satisfied by satisfying c21 = 0.

（ｄ）非線形測定方程式
上記（32）−（35）式をまとめて、簡単に（36）式の非線形測定方程式で示すように行列式で表すことができる。

但し、測定ベクトル

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

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

However, measurement vector

Is given by:

（ｅ）線形測定方程式（カルマンフィルタの観測式）
カルマンフィルタで使用するために（36）式を線形化すると、次式の線形測定方程式（カルマンフィルタの観測式）が得られる。

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

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

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

は図１３に示すように与えられる。通常走行状態では図１３の行列内の（1）の行を観測行列として使用し、静止状態では（1）の行と（2）の行を観測行列として使用する。もしもGPS受信機測定が同時に得られるときには（1）の行又は（1）の行と（2）の行に（3）の行を合わせて観測行列として使用する（特願２００７−２３３５１５参照）。さらに、センサー構成によっては（４）の行を加える。 (E) Linear measurement equation (Kalman filter observation formula)
When the equation (36) 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. 13 is used as an observation matrix, and in the stationary state, the row (1) and the row (2) are used as an 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.

測定値が得られたとき、以下の（40）−(42)式の反復計算を、所定の条件を満足するまで繰り返す。本発明では、所定の状態量、あるいは該状態量に応じた値について前回と今回との差分を演算し、該差分が設定値以下になったとき、あるいは、該差分が前回の差分より大きくなったとき、繰り返し演算を停止する。なお、以下の（40）、(41)、(42)式は、図７のカルマンフィルタ処理部１４の説明に際して使用した（7）、(9)、(10）式と同じである。

但し、上式において、

である。また、

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

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

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

本発明によれば、カルマンフィルタの特性を利用し、計算結果（走行軌跡精度）をほとんど変えることなく、処理負荷を約50％以上削減できる。
以上では、所定の状態量に応じた値、すなわち、センサー姿勢値について前回と今回との差分を演算し、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力する場合について説明したが、所定の状態量について前回と今回との差分を演算し、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力するように構成することもできる。 When the measured value is obtained, the iterative calculation of the following equations (40) to (42) is repeated until a predetermined condition is satisfied. In the present invention, the difference between the previous time and the current time is calculated for a predetermined state quantity or a value corresponding to the state quantity, and when the difference falls below a set value, or the difference becomes larger than the previous difference. When repeated, the computation is stopped. The following equations (40), (41), and (42) are the same as the equations (7), (9), and (10) used in the description of the Kalman filter processing unit 14 in FIG.

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 represented by (44a) in the normal running state, and is represented by (44b) 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.

According to the present invention, the processing load can be reduced by about 50% or more by using the characteristics of the Kalman filter and hardly changing the calculation result (running track accuracy).
In the above, a value corresponding to a predetermined state quantity, that is, a difference between the previous time and the current time is calculated for the sensor posture value, and when the difference becomes larger than the previous difference, or the difference becomes equal to or less than a set value. In this case, the calculation of the state quantity is stopped and the calculated state quantity is output. However, the difference between the previous time and the current time is calculated for the predetermined state quantity, and the difference becomes larger than the previous difference. Or when the difference falls below a set value, it is also possible to stop the repeated calculation of the state quantity and output the calculated state quantity.

本発明を適用できる車載ナビゲーション装置の位置検出部の構成図である。It is a block diagram of the position detection part of the vehicle-mounted navigation apparatus which can apply this invention. センサーボードに固定したセンサー座標系Xs-Ys-Zs及び加速度計3軸（Acc x, Acc y, Acc z）およびジャイロスコープ3軸（Gyro x, Gyro y, Gyro z）から成る6自由度の基本システムのセンサー説明図である。Basic 6-degree-of-freedom consisting of sensor coordinate system Xs-Ys-Zs fixed on sensor board, accelerometer 3 axes (Acc x, Acc y, Acc z) and gyroscope 3 axes (Gyro x, Gyro y, Gyro z) It is sensor explanatory drawing of a system. 車両に固定した座標系Xb-Yb-Zb及び地表固定座標系North-East-Down（NED座標系）の説明図である。It is explanatory drawing of coordinate system Xb-Yb-Zb fixed to the vehicle, and ground fixed coordinate system North-East-Down (NED coordinate system). 図１の位置検出部の全体の処理フローである。2 is an overall processing flow of the position detection unit in FIG. 1. センサー姿勢ピッチ角及びセンサー姿勢ヨー角説明図である。It is sensor posture pitch angle and sensor posture yaw angle explanatory drawing. 本発明のカルマンフィルタ処理における繰り返し演算制御の構成図である。It is a block diagram of repetitive calculation control in the Kalman filter processing of the present invention. カルマンフィルタ演算部の構成図である。It is a block diagram of a Kalman filter calculating part. 図６のカルマンフィルタ繰り返し演算停止制御部の構成図である。It is a block diagram of the Kalman filter repetition calculation stop control part of FIG. 状態量が収束したとみなして繰り返し演算を終了したときの本発明と従来技術の繰り返し演算回数の頻度分布説明図である。It is frequency distribution explanatory drawing of the frequency | count of repeated calculation of this invention and a prior art when it considers that the state quantity has converged and complete | finished repeated calculation. 走行軌跡説明図である。It is traveling locus explanatory drawing. 行列Ｆの構成要素説明図である。FIG. 10 is an explanatory diagram of components of a matrix F. 行列Gkの構成要素説明図である。FIG. 10 is an explanatory diagram of components of a matrix Gk. 行列Ｈの構成要素説明図である。FIG. 10 is an explanatory diagram of components of a matrix H. カルマンフィルタ処理の概要説明図である。It is outline | summary explanatory drawing of a Kalman filter process. センサー姿勢角c10の補正量dc10とイタレーション回数との関係特性A、補正量dc10の変化量とイタレーション回数の関係特性Bである。These are the relationship characteristic A between the correction amount dc10 of the sensor attitude angle c10 and the number of iterations, and the relationship characteristic B between the change amount of the correction amount dc10 and the number of iterations.

符号の説明Explanation of symbols

１４カルマンフィルタ演算部
１４a カルマンゲイン計算部
１４b 状態量補正部
１４c 状態量コバリアンス補正部
１４d 測定値コバリアンス発生部
１４e 観測行列発生部
２１補正状態量出力部
２１a ｃ10計算部
２１b 演算部
２１c 繰り返し演算終了決定部
２１d 目標値到達検出部
２１e 最大繰り返し検出部
２２カルマンフィルタ繰り返し演算停止制御部 14 Kalman filter calculation unit 14a Kalman gain calculation unit 14b State quantity correction unit 14c State quantity covariance correction unit 14d Measurement value covariance generation unit 14e Observation matrix generation unit 21 Correction state quantity output unit 21a c10 calculation unit 21b Calculation unit 21c Repetitive calculation end determination unit 21d Target value arrival detection unit 21e Maximum repetition detection unit 22 Kalman filter repetition calculation stop control unit

Claims

車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理における繰り返し演算制御方法において、
車両の位置、速度及びセンサーの姿勢を含む状態量を演算し、
演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回との差分を演算し、
該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力する、
ことを特徴とするカルマンフィルタ処理における繰り返し演算制御方法。 In the iterative calculation control method in the Kalman filter process for estimating the state quantity including the vehicle position, speed and sensor attitude,
Calculate the state quantity including vehicle position, speed and sensor attitude,
The difference between the previous time and the current time is calculated for a predetermined state quantity or a value corresponding to the state quantity among the calculated state quantities,
When the difference is greater than the previous difference, or when the difference is less than or equal to the set value, the state quantity repetition calculation is stopped and the calculated state quantity is output.
An iterative calculation control method in Kalman filter processing.

前記状態量に応じた値は、加速度センサーの姿勢ピッチ角およびジャイロセンサの姿勢ヨー角に依存する状態量に応じた値であることを特徴とする請求項１記載のカルマンフィルタ処理における繰り返し演算制御方法。 The method according to claim 1, wherein the value corresponding to the state quantity is a value corresponding to a state quantity depending on a posture pitch angle of an acceleration sensor and a posture yaw angle of a gyro sensor. .

車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理における繰り返し演算制御装置において、
車両の位置、速度及びセンサーの姿勢を含む状態量を演算する状態量演算部、
演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回の差分を演算する差分演算部、
該差分と前回の差分の大小を比較する比較部、
該差分が設定値以下になったか否かを検出する検出部、
該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、前記状態量の繰り返し演算を停止するよう制御すると共に、演算された状態量を出力するよう制御する制御部、
を備えたことを特徴とするカルマンフィルタ処理における繰り返し演算制御装置。 In the iterative calculation control device in the Kalman filter process for estimating the state quantity including the vehicle position, speed and sensor attitude,
A state quantity calculation unit for calculating a state quantity including a vehicle position, speed and sensor attitude;
A difference calculation unit that calculates a difference between a previous state and a current state for a predetermined state amount or a value corresponding to the state amount among the calculated state amounts,
A comparison unit that compares the difference with the previous difference,
A detection unit for detecting whether the difference is equal to or less than a set value;
When the difference becomes larger than the previous difference, or when the difference falls below a set value, control is performed to stop the repeated calculation of the state quantity, and control is performed to output the calculated state quantity. Control unit,
A repetitive calculation control device in Kalman filter processing, comprising: