JP5219547B2 - Car navigation system and navigation method - Google Patents
Car navigation system and navigation method Download PDFInfo
- Publication number
- JP5219547B2 JP5219547B2 JP2008039563A JP2008039563A JP5219547B2 JP 5219547 B2 JP5219547 B2 JP 5219547B2 JP 2008039563 A JP2008039563 A JP 2008039563A JP 2008039563 A JP2008039563 A JP 2008039563A JP 5219547 B2 JP5219547 B2 JP 5219547B2
- Authority
- JP
- Japan
- Prior art keywords
- sensor
- navigation
- vehicle
- axis
- sensor configuration
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Landscapes
- Gyroscopes (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Description
本発明は、車載ナビゲーションシステム及びナビゲーション方法に係わり、特に、任意のセンサー構成システムにおいても6自由度の基本システムのアルゴリズムを共通に利用できるようにし、かつセンサー軸数が減少しても大幅な精度減少がない、車載ナビゲーションシステム及びナビゲーション方法に関する。 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.
慣性航法システム(Inertial Navigation System:INS)は、通常3軸加速度計と3軸ジャイロスコープからなり、動体の6自由度、即ち3次元の並進運動と3次元の回転運動を同時に推定するために使用される。例えば、航空・宇宙機のナビゲーションは、かかる慣性航法システムを用いて、動体の6自由度、即ち3次元の並進運動と3次元の回転運動を同時に推定する。車両ナビゲーションにおいては車速パルスセンサーを使用することで、コストの高い慣性センサーを削減する場合が多い。 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受信機)と自律航法システムを、カルマンフィルタなどの最適確率アルゴリズムで結合することにより、両システムの性能上の問題を補間・解決する手法が考案された(特許文献1)。このようなシステムは複合ナビゲーションシステム(Integrated Navigation System)と呼ばれ、現在では車両ナビゲーションから航空宇宙ナビゲーションまで、幅広く利用されている。 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.
近年の車両ナビゲーションにおいては、GPS受信機単体を使用するローコスト商品から、GPS受信機と任意のセンサーを組み合わせて使用するハイエンド商品まで、様々なセンサー構成のナビゲーションシステムが存在する。しかし、一社でローコスト商品からハイエンド商品まで扱う場合、各商品におけるセンサーの組み合わせは異なり、通常、商品毎に異なるソフトウェア(SW)を開発する必要があり、開発とメンテナンスの手間が商品の種類の数に比例して増大する問題があった。
従って、センサーの組み合わせが異なる複数のシステムに共通に対応できる単一のプラットフォームソフトウェア(SW)を備え、しかもセンサー軸数が減少しても大幅な精度減少がないナビゲーション方法及び車載ナビゲーションシステムが要望されている。 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.
本発明の目的は、単一プラットフォームソフトウェア(SW)でセンサーの組み合わせが異なる複数のシステムに対応できるようにし、しかもセンサー軸数が減少しても大きな精度減少がないようにすることである。
本発明の別の目的は、任意のセンサー構成システムにおいて6自由度の基本システムのアルゴリズムを共通に利用できるようにすることである。
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.
本発明は車速パルスセンサー、加速度計や角速度計(ジャイロスコープ)およびGPS受信機の複数のセンサーを複合した車載ナビゲーションシステムおよびナビゲーション方法である。
・ナビゲーション方法
本発明のナビゲーション方法では、いくつかのセンサーの組み合わせで構成される複数のシステムの中から実際のセンサー構成システムを決定し、該決定した実際のセンサー構成システムと全センサーを備えた基本システムとに差異がある場合、欠けているセンサーの出力値を所定値に設定することにより、該実際のセンサー構成システムと基本システムとの差異を補完し、前記基本システムの制御アルゴリズムに基づいてカルマンフィルタ処理を行って車両の位置、速度及び姿勢の推定値を得る。前記基本システムはGPS受信機、車速パルスセンサー、加速度計3軸、ジャイロスコープ3軸を備えた6自由度のシステムである。
本発明のナビゲーション方法では、前記実際のセンサー構成システムを、システム設計時に予め設定し、あるいは、センサーから得られる信号をもとに、自動的に判別する。
また、本発明のナビゲーション方法では、前記差異を補完する補完手段を設け、該補完手段により、前記実際のセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、1)0またはホワイトノイズを採用し、あるいは、2)定数を採用し、あるいは3)他センサーの出力値を変数とする関数の関数値を採用する。
また、本発明のナビゲーション方法では、前記差異を補完する補完手段を設け、該補完手段により、前記実際のセンサー構成システムがロール角を推定するセンサーを持たないとき、ロール角推定量が発散するのをふせぐために、ロール角が0の測定量をカルマンフィルタに入力する。
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
In the navigation method of the present invention, an actual sensor configuration system is determined from a plurality of systems configured by combinations of several sensors, and the determined actual sensor configuration system and a basic system including all sensors are determined. If there is a difference, the output value of the missing sensor is set to a predetermined value to complement the difference between the actual sensor configuration system and the basic system, and Kalman filter processing is performed based on the control algorithm of the basic system. To obtain an estimate of the position, speed and attitude of the vehicle. 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.
In the navigation method of the present invention, the actual sensor configuration system is determined in advance at the time of system design or automatically based on a signal obtained from the sensor.
Further, in the navigation method of the present invention, a complementing means for complementing the difference is provided, and by the complementing means, 1) 0 or white noise is output as an output value of a sensor missing from the basic system in the actual sensor configuration system. Adopt, or 2) Adopt a constant, or 3) Adopt a function value of a function having the output value of another sensor as a variable.
Further, in the navigation method of the present invention, a complement means for complementing the difference is provided, and when the actual sensor configuration system does not have a sensor for estimating the roll angle, the roll angle estimation amount diverges by the complement means. In order to prevent this, the measured quantity with a roll angle of 0 is input to the Kalman filter.
・車載ナビゲーションシステム
本発明の車載ナビゲーションシステムは、車速パルスセンサー、加速度計や角速度計(ジャイロスコープ)およびGPS受信機を含む複数のセンサーを複合した車載ナビゲーションシステムであり、いくつかのセンサーの組み合わせで構成される複数のシステムの中から実際のセンサー構成システムを決定する手段、該決定した実際のセンサー構成システムと全センサーを備えた基本システムとに差異がある場合、欠けているセンサーの出力値を所定値に設定することにより、該実際のセンサー構成システムと基本システムとの差異を補完する補完部、補完された値及びシステムのセンサー出力信号を用いて自律航法計算する自律航法計算部、前記基本システムの制御アルゴリズムに基づいて自律航法計算結果及びGPS測定結果を用いてカルマンフィルタ処理を行い、車両の位置、速度及び姿勢の推定値を得る処理部を備えている。前記基本システムはGPS受信機、車速パルスセンサー、加速度計3軸、ジャイロスコープ3軸を備えた6自由度のシステムである。
本発明の車載ナビゲーションシステムは、更に、前記実際のセンサー構成システムをシステム設計時に予め設定する手段、あるいは、センサーから得られる信号をもとに実際のセンサー構成システムを自動的に判別する手段を備えている。
また、本発明の車載ナビゲーションシステムにおいて、前記補完手段は、前記実際のセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、1)0またはホワイトノイズを採用し、あるいは、2)定数を採用し、あるいは3)他センサーの出力値を変数とする関数の関数値を採用する。
また、本発明の車載ナビゲーションシステムにおいて、前記補完手段は前記実際のセンサー構成システムがロール角を推定するセンサーを持たないとき、ロール角推定量が発散するのをふせぐために、ロール角が0の測定量をカルマンフィルタに入力する。
・ In-vehicle navigation system
The vehicle-mounted navigation system of the present invention is a vehicle-mounted 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. If there is a difference between the determined actual sensor configuration system and the basic system including all sensors, the output value of the missing sensor is set to a predetermined value. A complementary unit that complements the difference between the actual sensor configuration system and the basic system, an autonomous navigation calculation unit that calculates an autonomous navigation using the complemented value and the sensor output signal of the system, and a control algorithm for the basic system Based on the results of autonomous navigation calculation and GPS measurement results It performs filter processing, and a processing unit for obtaining the position of the vehicle, the estimated value of the velocity and attitude. 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.
The in-vehicle navigation system of the present invention further includes means for presetting the actual sensor configuration system at the time of system design, or means for automatically determining the actual sensor configuration system based on a signal obtained from the sensor. ing.
In the in-vehicle navigation system of the present invention, the complementing means adopts 1) 0 or white noise as a sensor output value missing from the basic system in the actual sensor configuration system, or 2) a constant. 3) Adopt the function value of the function with the output value of another sensor as a variable.
Further, in the in-vehicle navigation system of the present invention, when the actual sensor configuration system does not have a sensor for estimating the roll angle, the complement means has a roll angle of 0 in order to prevent the estimated roll angle from diverging. Input the measured quantity to the Kalman filter.
本発明によれば、予め定義されたセンサー構成に応じて、基本の6自由度システムから欠けているセンサーの出力値として、力学的な知識に基づいて所定の値を採用することにより、自律航法計算部は常に同じ6自由度の基本システムの計算を行うことができる。結果として同じソフトウェア(SW)で異なるセンサー構成システムに対応することができ、開発工数を削減できる。また、個別のセンサー構成システムにだけ対応するアルゴリズムよりも、本発明では力学的に推定・補完したセンサー信号の分だけ多くの情報を使用することになり、自律航法計算の精度が向上する。このため本発明によれば、センサー軸数を削減しても、個別のセンサー構成システムにだけ対応するアルゴリズムに比べて精度減少が少ない。 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) 複合ナビゲーションシステムの基本的な構成
図1は、車両に搭載される本発明の複合ナビゲーションシステムの基本的な構成の例を示す概略ブロック線図である。
(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.
自律系の入力信号は、センサーボード10上の慣性センサーから得られる信号と、別のケーブルを通じて車体から直接ナビゲーションCPUに取り込まれる車速パルスである。慣性センサーとしては加速度計(加速度1〜3軸)、ジャイロスコープ(角速度1〜3軸)を採用し、車速パルスを発生するセンサーとしては車両が所定距離移動する毎に1個のパルスを発生する車速センサーを採用する。
The input signal of the autonomous system is a signal obtained from an inertial sensor on the
センサー・コンフィギュレーション(SC)・コントローラ20は、車載ナビゲーションシステムの実際のセンサー構成を設定部15から設定され、あるいは、センサー出力から取得した情報を評価することで該センサー構成を判別する。なお、センサー出力から取得した情報の評価とは、「センサー出力信号が異常値であるとき、システムは該当するセンサーを使わないセンサー構成であると自動判別すること」を指す。センサー出力信号が異常値であるとは、例えば、(1)常時一定電圧を出力している、あるいは、(2)バイアスや感度が仕様範囲を超えている、ということである。
The sensor configuration (SC)
SCコントローラ20は、前記判別されたセンサー構成に応じて、6自由度の基本システムから欠けているセンサーの出力値として、力学的な知識をもとに、定数(0を含む)、あるいはホワイトノイズ、あるいは他のセンサー出力値を変数とする関数の関数値を採用し、各センサーの出力値を自律航法計算部30に送る。また、SCコントローラ20は、前記判別されたセンサー構成がロール角を推定するセンサーをもたないとき、ロール角推定量が発散するのを防ぐために、ロール角=0の測定量をカルマンフィルタ補正部40に送る。このとき実際の車両では最大5度程度のロール角が生じるが、カルマンフィルタ補正部40では実際値との誤差はホワイトノイズとして取り扱う。
Depending on the determined sensor configuration, the
自律航法計算部20は常に同じ6自由度の基本システムのアルゴリズムを用いて、車両の現在の位置、速度及び姿勢を含む車両状態量を、高周波数で更新する。
The autonomous
GPS受信機50は、複数の人工衛星から距離と距離変化率に関する信号を受信することで、車両のアンテナの位置と車両速度の測定値を提供する。
The
演算部60は、自律航法計算部30により推定された車両状態量と、GPS受信機50から得られる位置・速度測定値を含む測定量との差分をとり、カルマンフィルタ補正部40はカルマンフィルタを用いて該差分に基づいて車両の現在の位置、速度及び姿勢を含む車両状態量の補正を行う。
The
(B)種々のシステムにおけるセンサー構成
図2(A)は、図1におけるセンサーボード10に固定した座標系Xs-Ys-Zsを示している。なお、以下の数式の中でセンサー(ボード)座標系に関する表現であることを、添え字の”s”によって表現することに注意すべきである。
(B) Sensor Configuration in Various Systems FIG. 2A shows a coordinate system Xs-Ys-Zs fixed to the
図2(B)は、(A)のセンサーボード10において、加速度計3軸(Acc x, Acc y, Acc z)およびジャイロスコープ3軸(Gyro x, Gyro y, Gyro z)から成る、即ち6自由度の基本システムのセンサー構成を示したものである。3軸加速度計Acc x〜Acc zは、センサー座標系の三つの座標方向(x,y,z)における加速度を検出し、また3軸ジャイロスコープGyro x〜Gyro zは、センサー座標系の三つの座標軸(x,y,z)周りの角速度P,Q,Rを検出する。
FIG. 2 (B) shows that the
図2(C)は、(A)のセンサーボード10において、加速度計3軸、ジャイロスコープ2軸から成るシステムのセンサー構成を示したものである。ここで、ジャイロスコープが2軸しか検出できないとき、システムのジャイロスコープはGyro xおよび Gyro zである。
FIG. 2 (C) shows a sensor configuration of a system that includes three axes of accelerometers and two axes of gyroscopes in the
図2(D)は、(A)のセンサーボード10において、加速度計3軸、ジャイロスコープ1軸から成るシステムのセンサー構成を示したものである。ここで、ジャイロスコープが1軸しか検出できないとき、システムのジャイロスコープはGyro zである。
FIG. 2 (D) shows the sensor configuration of a system comprising three axes of accelerometers and one axis of gyroscope in the
図3(A)は、図2(A)のセンサーボード10において、加速度計2軸、ジャイロスコープ1軸から成るシステムのセンサー構成を示したものである。ここで、加速度計が2軸しか検出できないとき、システムの加速度計はAcc x, Acc yであり、ジャイロスコープが1軸しか検出できないとき、システムのジャイロスコープはGyro zである。
FIG. 3 (A) shows the sensor configuration of a system comprising two axes of accelerometers and one axis of gyroscope in the
図3(B)は図2(A)のセンサーボード10において、加速度計1軸、ジャイロスコープ1軸ら成るシステムのセンサー構成を示したものである。ここで、加速度計が1軸しか検出できないとき、システムの加速度計はAcc xであり、ジャイロスコープが1軸しか検出できないとき、システムのジャイロスコープはGyro zである。
FIG. 3B shows a sensor configuration of a system including one axis of accelerometer and one axis of gyroscope in the
図3(C)は、図2(A)のセンサーボード10において、加速度計0軸(不使用)、ジャイロスコープ1軸から成るシステムのセンサー構成を示したものであり、ジャイロスコープが1軸しか検出できないとき、システムのジャイロスコープはGyro zである。
Fig. 3 (C) shows the sensor configuration of the system consisting of the
図4(A)は、車両に固定した座標系Xb-Yb-Zbを示している。以下の数式の中で車両固定座標系に関する表現であることを、添え字の”b”によって表現する。 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”.
図4(B)は、ある緯度、経度における、地表固定座標系North-East-Down(NED座標系)を示している。以下の数式の中でNED座標系に関する表現であることを、添え字の”n”によって表現する。 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)基本的作動プロセス
図5は、本発明に係るナビゲーションシステムの基本的作動プロセスの例を示しているフローチャートである。工程101において、車速パルスセンサー、慣性センサーである加速度計および角速度計をもとに予めいくつかのセンサー構成を定義する(図2、図3参照)。
(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
次に工程102において、SCコントローラ20は前記センサーから得られる信号をもとにナビゲーションシステムの前記センサー構成を判別する。
Next, in
工程103において、SCコントローラ20は前記判別したセンサー構成に応じて、6自由度の基本システムから欠けているセンサーの出力値として、力学的な知識をもとに、定数(0を含む)、あるいはホワイトノイズ、あるいは他のセンサー出力を変数とする関数の関数値を採用し、各センサーの出力を自律航法計算部30に送る。また、SCコントローラ20はロール角=0の測定量をカルマンフィルタ補正部40に送る。なお、ロール角=0の測定量をカルマンフィルタ補正部40に送らない場合もある。
In
工程104において、自律航法計算部30は、該センサー出力として採用した値と実際のセンサー出力値を基に、常に同じ6自由度基本システムのアルゴリズムを使って、車両状態量を高周波で更新する自律航法計算を行う。
In
工程105において、演算部60は自律航法計算部30により推定された車両状態量と、GPS受信機50から得られる位置・速度測定値を含む測定量との差分を演算し、カルマンフィルタ補正部40はカルマンフィルタを用いて該差分に基づいて車両の現在の位置、速度及び姿勢を含む車両状態量の補正を行う。
In
工程106において、複合自律/GPS受信機ナビゲーションシステムは、ナビゲーション精度を連続して最適化するために前記の工程102から工程105を反復実行する。
In
以下では、センサー構成によらず常に基本となる6自由度システムのアルゴリズムを使用して、車両状態量の推定計算を行う具体的な手法を説明する。 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.
(D)自律航法計算部における自律航法計算アルゴリズム
まず、基本となる6自由度システムの、高周波自律航法計算部30のアルゴリズムについて説明する。なお、以降の式においては、
で示すように変数の上にドットがついているものは、その変数vsの時間変化率を表しているものとする。
(D) Autonomous Navigation Calculation Algorithm in Autonomous Navigation Calculation Unit First, the algorithm of the high-frequency autonomous
Those with dot over the variable as shown, it is assumed that represents the time rate of change of the variable v s.
(a)速度方程式
速度方程式は次式
(b) 姿勢方程式
姿勢方程式は次式で表現される。
(4)式は、慣性航法システム(Inertial Navigation System: INS)の一般的な手法である以下の姿勢方程式
Equation (4) is the following attitude equation, which is a general method of the inertial navigation system (INS).
(c) 位置方程式
位置方程式は次式
(d) 固定パラメータ方程式
固定パラメータ方程式は次式
(e) 非線形状態方程式
上記(1)、(4)、(6)、(8)式をまとめて、簡単に(10)式の様に行列式で表すことができる
(f) 状態量更新
CPU上で(10)式を数値積分するには、例えば、次式の状態量更新式
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
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.
(g) 微小擾乱方程式
さて、GPS受信機などの測定値をもとに、カルマンフィルタ補正部40がカルマンフィルタ処理で状態量xを補正を行うためには、自律航法計算部30が状態量を非線形状態方程式により更新を行い、かつ、誤差量が線形方程式によって増加すると仮定して該誤差量(微小擾乱量)のコバリアンス値も更新する必要がある。そのために、(11)式の線形化を行い、微小擾乱方程式である行列式(12)を用意する。
は以下に与えられる微小擾乱ベクトルである。
の現在推定値、(13c)はホワイトノイズでモデル化された3軸ジャイロスコープ出力
に含まれる高周波ノイズと、3軸加速度計出力
に含まれる高周波ノイズである。それぞれのノイズの標準偏差(σ)はセンサースペックや、測定実験などによって得られ、ここではそれらの大きさを次のようにNで表す。
に関する微小擾乱量であり、
には、慣性航法システム(INS)の一般的な手法に基づき、以下の関係がある。
に関する微小擾乱量であり、
には、慣性航法システム(INS)の一般的な手法に基づき、以下の関係がある。
となる。
(12)式における線形システムを表わす行列
はそれぞれ図6、図7に示すように得られる。ただし、図6において、
Is the small disturbance vector given below.
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.
The amount of minute disturbance
Has the following relationship based on the general method of inertial navigation system (INS).
The amount of minute disturbance
Has the following relationship based on the general method of inertial navigation system (INS).
It becomes.
Matrix representing linear system in Eq. (12)
Are obtained as shown in FIGS. 6 and 7, respectively. However, in FIG.
(h) コバリアンスの更新
以上のように得られた行列
を使って、カルマンフィルタ処理の定式に従い、高周波で以下のように微小擾乱ベクトル
のコバリアンス行列(誤差共分散行列)を次式により更新する。
の相関値の期待値(E[ ])からなるコバリアンス行列であり、
に関するコバリアンス行列であり、それぞれ次式で表現される。
And according to the Kalman filter processing formula,
The covariance matrix (error covariance matrix) is updated by the following equation.
Is a covariance matrix consisting of the expected correlation value (E [])
The covariance matrix is expressed by the following equations.
(E)任意のシステムの基本システムから欠けているセンサーの出力決定法
ここで所定のセンサー構成システムにおいて、6自由度の基本システムから欠けているセンサーの出力値の決定法を説明する。SCコントローラ20は、力学的な知識をもとに、基本システムから欠けているセンサーの出力値として、定数、あるいはホワイトノイズ、あるいは他のセンサー出力を変数とする関数の関数値を採用して自律航法計算部30に入力する。また、SCコントローラ20は、ロール角=0の測定量をカルマンフィルタ補正部40に入力する。
(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
(a) 車速パルスセンサー、加速度計3軸、及びジャイロスコープ3軸を備えた基本システム:
かかる基本システムの場合、SCコントローラ20は各センサー出力を自律航法計算部30に入力し、自律航法計算部30は入力されたセンサー出力を用いて前記自律航法計算アルゴリズムを実行する((11)、(18)式の演算を実行する)。
(A) Basic system with vehicle speed pulse sensor,
In the case of such a basic system, the
(b) 車速パルスセンサー、加速度計3軸及びジャイロスコープ2軸を備えたシステム:
かかるシステムでは以下の(1)〜(3)に従ってセンサー出力を決定する。
(1)ジャイロスコープ2軸はxs及びzs軸を使用するものとする(図2(C))。
(2)ys軸ジャイロスコープ出力を0(ωys=Q=0)とし、
とする。即ち、車両モーションによるピッチ角速度を、平均値0(rad/s)と標準偏差0.05(rad/s)=2.9(deg/s)のホワイトノイズの加算であると仮定する。標準偏差値は6自由度の基本システムを用いて得られたデータを統計処理した値を使用している。後述する図8の螺旋駐車場を含むテストコースを実際に走行して採取したys軸ジャイロスコープ出力データの標準偏差値は0.054 (rad/s)であり、上記の標準偏差0.05(rad/s)は乗用車の車両ピッチ角モーションのおおよその代表的な値であると仮定できる。
(3)ys軸のジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。
(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.
(c) 車速パルスセンサー、加速度計3軸及びジャイロスコープ1軸を備えたシステム、あるいは 車速パルスセンサー、加速度計2軸及びジャイロスコープ1軸を備えたシステム:
かかるシステムでは以下の(1)〜(3)に従ってセンサー出力を決定する。
(1)加速度計2軸はxs及びys軸を使用するものする。また、ジャイロスコープ1軸は、zs軸を使用する(図2(D),図3(A))。
(2)xs軸ジャイロスコープ出力を0(ωxs=P=0)とし、
とする。即ち、車両モーションによるロール角速度の大きさを平均値0(rad/s)と、標準偏差0.05(rad/s)=2.9(deg/s)のホワイトノイズの加算である仮定する。標準偏差値は6自由度の基本システムを用いて得られたデータを統計処理した値を使用している。後述する図9の螺旋駐車場を含むテストコースを実際に走行して採取したxs軸ジャイロスコープ出力データの標準偏差値(0.049 rad/s)であり、上記の標準偏差0.05(rad/s)は乗用車の車両ロール角モーションのおおよその代表的な値であると仮定できる。
また、ys軸ジャイロスコープ出力を0(ωys=Q=0)とし、
とする。更に、
zs軸加速度計出力を‐9.8(m/s2)とし、
とする。即ち、車両モーションによるセンサー垂直方向加速度の大きさを平均値‐9.8(m/s2)と、 標準偏差0.3(m/s2)のホワイトノイズの加算であると仮定する。標準偏差値は6自由度システムを用いて得られたデータを統計処理した値を使用している。後述する図9の螺旋駐車場を含むテストコースを実際に走行して採取したzs軸加速度計出力データの標準偏差値は0.28(m/s2)であり、上記の標準偏差0.3(m/s2)は乗用車の車両垂直方向加速度モーションのおおよその代表的な値であると仮定できる。
(3)xs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。また、ys軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。さらに、zs軸加速度計出力に含まれるバイアス
で固定する。
(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
(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 2 )
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 2 ) and a standard deviation of 0.3 (m / s 2 ). 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 2 ), and the standard deviation value is 0.3 (m / s). It can be assumed that s 2 ) 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.
(d) 車速パルスセンサー、加速度計1軸、及びジャイロスコープ1軸のシステム:
かかるシステムでは以下の(1)〜(4)に従ってセンサー出力を決定する。
(1)加速度計1軸はxs軸をするものとし、ジャイロスコープ1軸はzs軸を使用するものとする(図3(B)参照)。
(2)(c)のシステムと同様にxs軸ジャイロスコープ出力を0(ωxs=P=0)とし、
とする。また、ys軸ジャイロスコープ出力を0(ωys=Q=0)とし、
とする。更に、ys軸加速度計出力を、
車速パルスから得られる速度(vxb_obs)×zs軸ジャイロスコープ出力ωzs(=R)
とする(掛け算の結果の単位はm/s2)。速度vxb_obsについては後述するが、ys軸加速度計出力は、zs軸ジャイロスコープ出力ωzsを変数とする関数の関数値であるといえる。また
とする。即ち、車両モーションによるセンサー横方向加速度の大きさを、vxb_obs×ωzsの平均値と標準偏差0.7(m/s2)のホワイトノイズの加算であると仮定する。平均値を表す上式は、力学的に平面内回転運動において、横方向加速度が前方向速度と角速度の積で得られることから、定性的に推定されたものである。それ以外の横方向加速度出力はロール角による重力方向成分を含む。標準偏差値は6自由度システムを用いて得られたデータを統計処理した値を使用している。後述する図9の螺旋駐車場を含むテストコースを実際に走行して採取したys軸加速度計出力データとvxb_obs×ωzsとの差の標準偏差値は0.72(m/s2)であり、上記の標準偏差0.7(m/s2)は、乗用車の横方向加速度モーションのおおよその代表的なモデルと仮定できる。また、(c)のシステムと同様にzs軸加速度計出力を‐9.8(m/s2)とし、Nwazs=0.3(m/s2)とする。
(3)xs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。また、ys軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。さらに、ys軸加速度計、zs軸加速度計出力に含まれるバイアス
で固定する。
(4)ロール角=0の測定量を後述のカルマンフィルタ補正部40で使用する。
(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 2 ). 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 2 ). 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 2 ). Yes, the above standard deviation of 0.7 (m / s 2 ) 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 2 ), and N wazs = 0.3 (m / s 2 ).
(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
(e) 車速パルスセンサー、加速度計0軸、及びジャイロスコープ1軸のシステム:
かかるシステムでは以下の(1)〜(4)に従ってセンサー出力を決定する。
(1)ジャイロスコープ1軸はzs軸を使用するものとする(図3(C)。
(2) (c)のシステムと同様にxs軸ジャイロスコープ出力を0(ωxs=P=0)とし、
とし、ys軸ジャイロスコープ出力を0(ωys=Q=0)とし、
とする。また、xs軸加速度計出力を0とし、
とする。即ち、車両モーションによるセンサー前方向加速度の大きさを、平均値0(m/s2)と、 標準偏差0.8(m/s2)のホワイトノイズの加算であると仮定する。この標準偏差値は坂道の多さやドライバーの加減速によって0.5〜1.0(m/s2)と異なる値であるが、ここでは後述する図9の螺旋駐車場を含むテストコースを実際に走行して採取したxs軸加速度計出力データの標準偏差値0.84(m/s2)を採用する。
更に、(d)のシステムと同様にys軸加速度計出力を
車速パルスから得られる速度(vxb_obs)×zs軸ジャイロスコープ出力ωzs(=R)
とし(掛け算の結果の単位はm/s2)、
とする。また、zs軸加速度計出力を‐9.8(m/s2)とし、
とする。
(3)xs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。また、ys軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。さらに、xs軸加速度計、ys軸加速度計、zs軸加速度計出力に含まれるバイアス
で固定する。
(4)ロール角=0の測定量を、後述のカルマンフィルタ補正部40で使用する。
以上の(1)−(4)におけるパラメータの設定例は代表的なものであり、設計者が意図に応じてデザインパラメータとして変化させることも可能である。ただしオーダーが変わるほどの変化でない限り、システム性能はさほど変わらない。
(E) Vehicle speed pulse sensor,
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
(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 2 ) and a standard deviation of 0.8 (m / s 2 ). This standard deviation value is different from 0.5 to 1.0 (m / s 2 ) 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 2 ) 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 2 )
And Also, set the z s- axis accelerometer output to -9.8 (m / s 2 )
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
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)カルマンフィルタ補正部の計算
次に、低周波の測定値入力に伴う、カルマンフィルタ補正部40の計算方法について説明する。なお、以下の測定方程式で、
のように表されているのは、ホワイトノイズでモデル化された測定ノイズであり、その標準偏差は
の様に表されている。
(F) Calculation of Kalman Filter Correction Unit Next, a calculation method of the Kalman
Is the measurement noise modeled with white noise, and its standard deviation is
It is expressed as
(a)通常走行状態
通常走行状態においては、次式の測定方程式
vxb_obs = 車速パルス間距離をパルス間隔で割ることで得られ、vyb_obs = 0、vzb_obs = 0である。これらの標準偏差は例えば
である。ただし、これらの値はデザイン値であり、ここに記した値は目安である。
(A) Normal driving condition In the normal driving condition, the following measurement equation
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)静止状態
車速パルスが2秒以上入ってこないとき、車両が静止状態にあると判定する。静止状態において、測定値及びそれらの標準偏差は
であるとし、これに加えて以下の測定方程式を用いる。
In addition to this, the following measurement equation is used.
(c)GPS受信機測定状態
以下のGPS受信機測定値が得られるときには、上記通常走行状態と静止状態のいずれかの測定に加え、以下のGPS受信機測定結果を利用する。
ロール角=0の情報が送られてきたときには、次の測定方程式を使用する。
c21 = sin(センサーロール角)・cos(センサーピッチ角)
に相当し、c21 = 0を満たすことで、ロール角=0が自動的に満たされるからである。
When information on roll angle = 0 is sent, the following measurement equation is used.
c 21 = sin (sensor roll angle) ・ cos (sensor pitch angle)
C 21 = This is because satisfying 0 automatically satisfies the roll angle = 0.
(d)非線形測定方程式
上記(21)−(24)式をまとめて、簡単に(25)式の非線形測定方程式で示すように行列式で表すことができる。
は以下のように与えられる。
Is given by:
(e)線形測定方程式(カルマンフィルタの観測式)
カルマンフィルタで使用するために(25)式を線形化すると、次式の線形測定方程式(カルマンフィルタの観測式)が得られる。
は以下のように与えられる。
は図8に示すように与えられる。通常走行状態では図8の行列内の(1)の行を観測行列として使用し、静止状態では(1)の行と(2)の行を観測行列として使用する。もしもGPS受信機測定が同時に得られるときには(1)の行又は(1)の行と(2)の行に(3)の行を合わせて観測行列として使用する(特願2007−233515参照)。さらに、センサー構成によっては(4)の行を加える。
(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.
Is given by:
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.
測定値が得られたとき、以下の(29)−(31)式の反復計算をi = 0 から n回行う。通常n = 2回以上反復すれば反復しないときに比べ大幅に精度が向上し、10回以上行う必要はあまりない。測位が得られるたびに反復計算を行うこの方法は、反復拡張カルマンフィルタ(Iterated Extended Kalman Filter)と呼ばれる周知の方法である(以下の文献参照)。
Gelb, A., Applied Optimal Estimation, M.I.T. Press, Cambridge, MA, 1974, pp. 182-91.
は測定値に関するコバリアンス行列であり、使用する
に応じて変化する。すなわち、コバリアンス行列は通常走行状態であれば(33a)で表現され、静止状態であれば(33b)式で表現される。
Gelb, A., Applied Optimal Estimation, MIT Press, Cambridge, MA, 1974, pp. 182-91.
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.
(G)実験結果
図9から図13は、本発明の有効性を示す実験結果を示している。図14は比較のための、本発明を使用しない現行ナビゲーションシステムの結果を示している。実験の方法は、まず路上走行によって各センサーとGPS受信機のデータを収集し、ついで机上パーソナルコンピュータ(PC)で本発明のナビゲーションシステムを構成し、路上収集センサーデータをPCに入力することでシミュレーションを行う、というプロセスからなる。様々な場面での路上走行テストの中から、システム毎の差異が大きく表れる螺旋駐車場走行を利用して実験結果を比較している。これは螺旋駐車場内においてGPS受信機に信号が届かず、補正がかからない状態で上り下りしながら何度も回転するため、方位角推定には最も厳しい条件であり、自律航法システムの性能差が顕著に現れるためである。
図9から図14は全て平面図であり、円弧を描いて見えるところが螺旋駐車場である。実験走行は、各図の(A)に示される螺旋駐車場を2回(コース1とコース2)、続いて各図の(B)に示される螺旋駐車場を2回(コース3とコース4)、図中1−2−3−4の順に連続に走行している。連続した点の軌跡は本発明の複合システムの推定軌跡であり、まばらな点の軌跡は、GPS受信機のみによる推定した位置の軌跡である。いずれの図でも、推定軌跡はGPS受信機軌跡に引き寄せられて道路リンク(直線)上にあるが、螺旋駐車場の入り口、出口での方位角に注目してもらいたい。なお、駐車場前後で車両は直線道路上を走行している。
(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 (
図9は、車速パルスセンサー、3軸加速度計、3軸ジャイロのセンサー構成からなる基本システム(6自由度システム:Acc3 Gyro3システム)の結果である推定軌跡を示している。推定軌跡1〜4は、駐車場への進入方位角と脱出方位角が一直線になり、実際の走行軌跡と同じであることが判る。
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
図10は、車速パルスセンサー、3軸加速度計、2軸ジャイロのセンサー構成からなるシステム(Acc3 Gyro2システム)の結果を示している。6自由度の基本システムとほぼ遜色のない結果が得られていることが確認できる。 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.
図11は、車速パルスセンサー、3軸あるいは2軸加速度計、1軸ジャイロのセンサー構成からなるシステム(Acc3 Gyro1システムまたはAcc2 Gyro1システム)の結果を示している。ここでは前述のように、加速度計が3軸でも2軸でも、同じアルゴリズムを用いている。このシステムでは出口方位角において10度〜20度の誤差が生じている推定軌跡1,3,4が確認できる。
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
図12は、車速パルスセンサー、1軸加速度計、1軸ジャイロのセンサー構成(Acc1 Gyro1システム)からなるシステムの結果を示している。図11のシステムと同様に出口方位角において10度〜15度の誤差が生じている推定軌跡1,3,4が確認できる。
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
図13は、車速パルスセンサー、0軸加速度計、1軸ジャイロのセンサー構成からなるシステム(Acc0 Gyro1システム)の結果を示している。出口方位角において10度〜15度の誤差が生じている推定軌跡3,4が確認できる。なお、このセンサー構成システムはピッチ角推定性能をもたず、高度に関する推定は自律航法で行わず、GPS受信機の測位データによってのみ補正が行われる。
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.
図14は比較のため、本発明を使用していない、車速パルスセンサー、1軸加速度計、1軸ジャイロのセンサー構成からなる現行ナビゲーション商品の結果を示している。このシステムは車両のロール角、ピッチ角の影響は考慮されておらず、かつ、カルマンフィルタ処理を用いないため感度誤差補正、バイアス補正が不正確になり、螺旋駐車場のような厳しいケースで、出口方位角が大きく不正確になることを示している。すなわち、実際の走行軌跡は進入方位角と脱出方位角が一直線であるが、図14では、推定された進入方位角と脱出方位角が一直線にならず、脱出方位が大幅にずれ、走行道路から大幅にずれている。 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, but in FIG. 14, the estimated approach azimuth angle and the escape azimuth angle are not in a straight line, and the escape azimuth is greatly deviated and There is a significant shift.
図15から図19は、シミュレーション計算中に使用した、コバリアンス行列Pの対角要素(σ2に相当)を用いて、位置と方位角の推定値に関する2σ値(2×標準偏差σ=96%誤差範囲)をプロットしたもので、横軸は経過時間(sec)、縦軸は誤差距離(m)及び誤差角度(deg)である。この方法は、一つの走行データを使って統計的な精度を論じるための手段であり、ホワイトノイズの仮定が正しいときに成り立つものである。図15−図19は、それぞれ、対応する図9−図13における螺旋駐車場走行コース3における入り口から出口付近までの位置誤差2σ値(図15−図19の(A))、及び方位角誤差2σ値(図15−図19の(B))を示しており、駐車場内でGPS受信機補正が掛からないために右上がりになっている。なお、駐車場外でGPS受信機の補正を受けた際は、いずれのシステム精度もGPS受信機精度以上に収束し、システム毎の差異は表れない。
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 σ 2 ) 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
図15は、6自由度の基本システムの螺旋駐車場走行3における精度結果を示している。位置精度、方位角精度ともに5つのシステムの中で最高(2σ値が最小)であり、システムの中の基準精度を示す。 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.
図16は、Acc3 Gyro2システムの螺旋駐車場走行コース3における精度結果を示している。6自由度の基本システムに比べ、平面位置精度(N、E)はほぼ同等であり、高さ誤差が1.5倍程度大きくなるものの、方位角精度もほぼ同等であることが確認できる。
FIG. 16 shows an accuracy result in the spiral parking
図17は、Acc3 Gyro1システムの螺旋駐車場走行コース3における精度結果を示している。6自由度の基本システムに比べ、平面位置精度(N、E)はほぼ同等、高さ誤差が約2倍、方位角誤差が約2倍程度であることが確認できる。
FIG. 17 shows an accuracy result in the spiral parking
図18は、Acc1 Gyro1システムの螺旋駐車場走行コース3における精度結果を示している。6自由度の基本システムに比べ、平面位置精度(N、E)はほぼ同等、高さ誤差が約2倍、方位角誤差が約1.5倍程度であることが確認できる。この時点で、本案の具体例を用いた場合、安価なAcc1 Gyro1システムの方がAcc3 Gyro1システムよりも高性能であることがわかった。
FIG. 18 shows an accuracy result in the spiral parking
図19は、Acc0 Gyro1システムの螺旋駐車場走行コース3ににおける精度結果を示している。6自由度の基本システムに比べ、平面位置精度(N、E)はほぼ同等に対して、高さ誤差が約7倍と極端に悪いのは、Acc0 Gyro1システムは自律航法で高さ方向をトラックしないからである。一方、方位角誤差は約1.5倍程度と、Acc1 Gyro1システムとほぼ同等であることが確認できる。
FIG. 19 shows the accuracy results in the spiral parking
図20は前記精度性能をまとめた図表である。右端列に目安となるコストを示したが、これは、現在の一般的な車両スペックセンサーの量産値段をもとにして次式、
(加速度計1軸¥500)×軸数+(ジャイロ1軸¥700)×軸数
から概算したものである。なお、左端のAiGjはACCi Gyrojと同じである。
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 (
なお、本発明方法と異なり、カルマンフィルタやパーティクルフィルタといった確率最適理論を使用しない方法では、図20のような理論精度とコストを関連付けた議論ができず、このため、強い商品設計が困難である。 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.
上述してきたように、本発明によれば、センサーから取得した情報を評価することで現在のセンサー構成を判別し、判別したセンサー構成と基本となる6自由度の基本システムとの差異を補完する手段を講じ、センサー構成によらず常に前記6自由度の基本システムのアルゴリズムを使用して、カルマンフィルタ処理を行い車両状態量の推定計算を行う。結果として、複数センサー構成に対して1つのソフトウェアで対応できるので開発効率が上がる。また、個別のセンサー構成にだけ対応するアルゴリズムよりも、力学的に推定・補間したセンサー信号の分だけ多くの情報を使用することになり、自律航法計算の精度が向上する。このためセンサー軸数を削減しても、個別のセンサー構成にだけ対応するアルゴリズムよりも精度減少が少ない。 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.
10 センサーボード
20 SCコントローラ
30 自律航法計算部
40 カルマンフィルタ補正部
50 GPS受信機
60 演算部
DESCRIPTION OF
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,
The actual sensor configuration system is determined from multiple systems composed of several sensor combinations.
If there is a difference in the basic system with the actual sensor configuration system and all sensors were the determined by setting the output value of the missing sensor to a predetermined value, and the actual sensor configuration system and the base system To compensate for the differences,
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.
ことを特徴とする請求項1記載のナビゲーション方法。 The basic system is a 6-degree-of-freedom system including a GPS receiver, a vehicle speed pulse sensor, an accelerometer 3-axis, and a gyroscope 3-axis.
The navigation method according to claim 1, wherein:
ことを特徴とする請求項1または2記載のナビゲーション方法。 The actual sensor configuration system is preset at the time of system design, or automatically determined based on a signal obtained from the sensor,
The navigation method according to claim 1 or 2, wherein
ことを特徴とする請求項1又は2記載のナビゲーション方法。 Complement means for complementing the difference is provided, and the complement means adopts 0 or white noise as the output value of the sensor that is missing from the basic system in the actual sensor configuration system.
The navigation method according to claim 1 or 2, characterized in that
ことを特徴とする請求項1又は2記載のナビゲーション方法。 Provided complementary means to supplement the difference, said complementary means, as an output value of the sensor missing from the base system in the actual sensors configuration system, employing a constant,
The navigation method according to claim 1 or 2, characterized in that
ことを特徴とする請求項1又は2記載のナビゲーション方法。 Complement means for complementing the difference is provided, and 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 actual sensor configuration system. ,
The navigation method according to claim 1 or 2, characterized in that
ことを特徴とする請求項1又は2記載のナビゲーション方法。 Complement means for complementing the difference is provided, and the complement means has a roll angle of 0 to prevent the estimated roll angle from diverging when the actual sensor configuration system does not have a sensor for estimating the roll angle. To input the measured amount of to the Kalman filter,
The navigation method according to claim 1 or 2, characterized in that
いくつかのセンサーの組み合わせで構成される複数のシステムの中から実際のセンサー構成システムを決定する手段、
該決定した実際のセンサー構成システムと全センサーを備えた基本システムとに差異がある場合、欠けているセンサーの出力値を所定値に設定することにより、該実際のセンサー構成システムと基本システムとの差異を補完する補完部、
補完された値及びシステムのセンサー出力信号を用いて自律航法計算する自律航法計算部、
前記基本システムの制御アルゴリズムに基づいて自律航法計算結果及びGPS測定結果を用いてカルマンフィルタ処理を行い、車両の位置、速度及び姿勢の推定値を得る処理部、
を備えたことを特徴とする車載ナビゲーションシステム。 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.
Means for determining an actual sensor configuration system from among a plurality of systems composed of combinations of several sensors;
If there is a difference in the basic system with the actual sensor configuration system and all sensors were the determined by setting the output value of the missing sensor to a predetermined value, and the actual sensor configuration system and the base system Complement part to compensate for the difference,
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:
ことを特徴とする請求項8記載の車載ナビゲーションシステム。 The basic system is a 6-degree-of-freedom system including a GPS receiver, a vehicle speed pulse sensor, an accelerometer 3-axis, and a gyroscope 3-axis.
The in-vehicle navigation system according to claim 8.
を備えたことを特徴とする請求項8または9記載の車載ナビゲーションシステム。 Means for presetting the actual sensor configuration system at the time of system design, or means for automatically determining the actual sensor configuration system based on a signal obtained from the sensor;
An in-vehicle navigation system according to claim 8 or 9, further comprising:
ことを特徴とする請求項8又は9記載の車載ナビゲーションシステム。 The complementary means employs 0 or white noise as the output value of the sensor that is missing from the basic system in the actual sensor configuration system.
The in-vehicle navigation system according to claim 8 or 9, characterized in that.
ことを特徴とする請求項8又は9記載のナビゲーションシステム。 The complement means adopts a constant as the output value of the sensor that is missing from the basic system in the actual sensor configuration system,
10. The navigation system according to claim 8 or 9, wherein:
ことを特徴とする請求項8又は9記載の車載ナビゲーションシステム。 The complement means employs 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 actual sensor configuration system.
The in-vehicle navigation system according to claim 8 or 9, characterized in that.
ことを特徴とする請求項8又は9記載の車載ナビゲーションシステム。 When the actual sensor configuration system does not have a sensor for estimating the roll angle, the complementing means inputs a measurement value with a roll angle of 0 to the Kalman filter in order to prevent the roll angle estimator from diverging.
The in-vehicle navigation system according to claim 8 or 9, characterized in that.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2008039563A JP5219547B2 (en) | 2008-02-21 | 2008-02-21 | Car navigation system and navigation method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2008039563A JP5219547B2 (en) | 2008-02-21 | 2008-02-21 | Car navigation system and navigation method |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2009198279A JP2009198279A (en) | 2009-09-03 |
JP5219547B2 true JP5219547B2 (en) | 2013-06-26 |
Family
ID=41141938
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2008039563A Active JP5219547B2 (en) | 2008-02-21 | 2008-02-21 | Car navigation system and navigation method |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5219547B2 (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2014006131A (en) * | 2012-06-22 | 2014-01-16 | Pioneer Electronic Corp | Speed calculation device, speed calculation method, speed calculation program, and recording medium |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20110035609A (en) * | 2009-09-30 | 2011-04-06 | 삼성전자주식회사 | Apparatus and method for sensing motion |
CN103217981A (en) * | 2013-04-18 | 2013-07-24 | 东南大学 | Four-rotor aircraft speed control method based on integral variable structure control |
CN106873612A (en) * | 2017-02-24 | 2017-06-20 | 宁波工程学院 | Electrodynamic balance car attitude method for quick |
JP7148092B2 (en) * | 2020-11-09 | 2022-10-05 | 株式会社オプティマインド | PARKING/STOP POSITION DETERMINATION DEVICE AND PARKING/STOP POSITION DETERMINATION PROGRAM |
CN114659488B (en) * | 2022-02-08 | 2023-06-23 | 东风悦享科技有限公司 | High-dynamic vehicle attitude estimation method based on error Kalman filtering |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2627961B2 (en) * | 1989-12-27 | 1997-07-09 | 松下電器産業株式会社 | Traffic information collection device |
JP3753833B2 (en) * | 1997-03-27 | 2006-03-08 | アジア航測株式会社 | Road linear automatic surveying equipment |
JP2000055678A (en) * | 1998-08-04 | 2000-02-25 | Denso Corp | Current position detecting device for vehicle |
JP2002318274A (en) * | 2001-02-13 | 2002-10-31 | Mazda Motor Corp | Instrument and method for measuring car body movement and recording medium where the method is recorded |
JP2003262530A (en) * | 2002-03-07 | 2003-09-19 | Mitsubishi Electric Corp | Navigation apparatus and method thereof, and program |
-
2008
- 2008-02-21 JP JP2008039563A patent/JP5219547B2/en active Active
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2014006131A (en) * | 2012-06-22 | 2014-01-16 | Pioneer Electronic Corp | Speed calculation device, speed calculation method, speed calculation program, and recording medium |
Also Published As
Publication number | Publication date |
---|---|
JP2009198279A (en) | 2009-09-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5328252B2 (en) | Position detection apparatus and position detection method for navigation system | |
JP5036462B2 (en) | Navigation system and navigation method | |
US9026263B2 (en) | Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis | |
Fakharian et al. | Adaptive Kalman filtering based navigation: An IMU/GPS integration approach | |
EP3321631B1 (en) | A inertial and terrain based navigation system | |
EP2555017B1 (en) | Vehicle navigation on the basis of satellite positioning data and vehicle sensor data | |
US7979231B2 (en) | Method and system for estimation of inertial sensor errors in remote inertial measurement unit | |
EP1585939B1 (en) | Attitude change kalman filter measurement apparatus and method | |
JP4781300B2 (en) | Position detection apparatus and position detection method | |
JP7036080B2 (en) | Inertial navigation system | |
JP5419665B2 (en) | POSITION LOCATION DEVICE, POSITION LOCATION METHOD, POSITION LOCATION PROGRAM, Velocity Vector Calculation Device, Velocity Vector Calculation Method, and Velocity Vector Calculation Program | |
US8560234B2 (en) | System and method of navigation based on state estimation using a stepped filter | |
JP5602070B2 (en) | POSITIONING DEVICE, POSITIONING METHOD OF POSITIONING DEVICE, AND POSITIONING PROGRAM | |
JP5219547B2 (en) | Car navigation system and navigation method | |
JP5164645B2 (en) | Method and apparatus for repetitive calculation control in Kalman filter processing | |
JP2009019992A (en) | Position detection device and position detection method | |
CN112683269A (en) | MARG attitude calculation method with motion acceleration compensation | |
CN117053782A (en) | Combined navigation method for amphibious robot | |
CN113566850B (en) | Method and device for calibrating installation angle of inertial measurement unit and computer equipment | |
JP7028223B2 (en) | Self-position estimator | |
WO2020021867A1 (en) | Geoid measurement method, geoid measurement apparatus, geoid estimation device, and geoid calculation data collection device | |
KR101941009B1 (en) | Attitude and heading reference system and unmaned vehicle including the attitude and heading refernce system | |
CN112577513A (en) | State quantity error determination method and vehicle-mounted terminal | |
US8812235B2 (en) | Estimation of N-dimensional parameters while sensing fewer than N dimensions | |
KR101916908B1 (en) | Localization system and unmaned vehicle including the localization system |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20101220 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20120217 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20120627 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20120703 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20120823 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20121030 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20121226 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20130305 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20130305 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20160315 Year of fee payment: 3 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5219547 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 Free format text: JAPANESE INTERMEDIATE CODE: R150 |