JPH03188318A - 方位検出装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〈産業上の利用分野〉 本発明は方位検出装置に関し、さらに詳細にいえば、地
磁気センサを使用し、この他に移動体の移動方向を検出
して旋回角速度を知る旋回角速度センサ（例えば、光フ
アイバジャイロ、機械式ジャイロ、振動ジャイロ、ガス
レートジャイロ）を使用して移動体のヨ一方向の方位を
検出する方位検出装置に関するものである。

〈従来の技術〉 従来から、道路交通網の任意の箇所を走行している車両
、あるいは航空路を航行する航空機、海路を航行する船
舶等（以下「車両」を想定して説明を進める。車両の「
走行」、航空機、船舶の「航行」をまとめていうときは
「走行」という）の位置を検出する方式として、距離セ
ンサと、方位センサ（地磁気センサ、または旋回角速度
センサのいずれか１つ）と、両センサからの出力信号に
必要な処理を施す処理装置とを具備し、車両の走行に伴
なって生ずる距離変化量Δノ、および方位θ（地磁気セ
ンサの場合）、もしくは方位変化量Δθ（旋回角速度セ
ンサの場合）を用いて移動体の現在位置データを得る推
測航法（ＤｅａｄＲｅｃｋｏｎｉｎｇ　）が提案されて
いる。この方式は、Δノおよびθに基いて、例えばΔＪ
の東西方向成分ΔＸ（−ΔＪＸｃｏｓθ）、および南北
方向成分Δｙ（−ΔＪＸｓｉｎθ）を算出し、従前の位
置出力データ（Ｐｘ’、　Ｐｙ’）に対して上記各成分
ΔＸ、Δｙを加算することにより、現在の位置出力デー
タ（Ｐｘ、Ｐｙ）を求める方式であるが、方位センサが
必然的に有している誤差のために、得られる現在位置デ
ータに含まれる誤差が累積されてしまうという欠点があ
る。

すなわち、方位センサが、地磁気を検出して移動体の絶
対方位を知る地磁気センサである場合には、地磁気方位
センサは微弱な地球磁界の強さを検出するものであり、
移動体本体が着磁してしまうとその出力データには誤差
が発生する。この誤差を打ち消すために地磁気方位セン
サの初期化処理が行われるが、車両であればその走行中
、特に踏切、電カケープル埋設場所、鉄橋、防音壁のあ
る高速道路や高層ビルの谷間を通過する時等にしばしば
外部からの強電磁界の影響を受けて車体の着磁量が変化
することにより、再度誤差が発生することがある。した
がって、このような磁界の乱れを含んだ地磁気センサ出
力データを的確に検出して排除しなければ、正しい方位
を求めることができない。

一方、旋回角速度センサを使用する場合には、例えば、
方位変化が所定値以上となった時、電源（イグニション
）オン時、極低速走行時、または、山道などの悪路走行
が検出された時等には、センサの出力データにノイズが
多く現れることが知られており、これらのデータをその
まま使用すると方位検出精度が低下する。

そこで、方位センサとして、旋回角速度センサと、地磁
気センサとの２つの方位センサを併用し、旋回角速度セ
ンサの出力データと、地磁気センサの出力データとのい
ずれかの信頼性が低下した場合に他のデータで補うよう
にした方位検出装置が考えられる。

この方位検出装置に適用しうる１つの方法は、旋回角速
度センサの出力データに含まれるノイズと、地磁気セン
サの出力データに含まれるノイズとを、ともに正規分布
に従うランダムノイズと考えて、カルマンフィルタの理
論を用いて、ノイズの予測値が少ないほうのデータすな
わち信頼性の多いと思われるデータに重みをおいて最終
的な方位を推定する方法である。

ところが、旋回角速度センサでは、直線走行中でセンサ
出力が０であるべき時でも、温度や湿度の影響を受けて
幾らかの出力（オフセット）が発生するという傾向があ
る。このオフセット出力は、累積するという性質を有す
るので実際の走行方向からずれた方向を検知してしまう
ことになる。

しかも、オフセット値、およびその時間変化特性は、温
度や湿度、旋回角速度センサの種類によって異なる。ま
た、同種の旋回角速度センサであっても固体差を反映す
る。

さらに、このオフセット出力は、旋回角速度センサ出力
データのランダムノイズとして検出することができない
。この理由は、ランダムノイズの場合、平均値が０にな
るのでデータの分散値をとることにより検出できるが、
オフセット出力では、平均値が０とならないからである
。

したがって、カルマンフィルタの理論を用いて、ノイズ
の少ないほうのデータすなわち信頼性の多いほうのデー
タに重みをおいて最終的な方位を推定する場合にあって
は、オフセットに対する何らかの補正処理が望まれる。

本発明の目的は、旋回角速度センサと地磁気センサの出
力データを取り込み、それらの値と、過去の推定方位か
ら移動体の現在の方位を算出し、もって、移動体の現在
位置を求める方位検出装置において、上記旋回角速度セ
ンサの出力データの信頼性を判定する場合、実際に使わ
れている旋回角速度センサに生じるオフセットのずれを
含めた形で信頼性を判定し、移動体の現在の方位を正確
に推定することができる方位検出装置を提供することに
ある。

く課題を解決するための手段〉 上記の目的を達成するための本発明の方位検出装置は、
第１図に示すように、旋回角速度センサのオフセット値
を移動体停止時の実測オフセット値と、停止時から次の
停止時までに計測した期間とに基づき、時間の関数ｏ　
（ｔ）として予測するオフセット値予測手段１５と、地
磁気センサの出力データθ８のノイズ成分σＨを記憶す
るとともに、旋回角速度センサの出力データΔθＧのノ
イズ成分σＧを記憶し、上記予測されたオフセットｏ　
（ｔ）を用いて旋回角速度センサの出力データΔθＧの
見掛けのノイズ成分を式 ％式％（）） により計算するノイズ算出手段１２と、上記ノイズ成分
を用いてカルマンフィルタゲインＫを算出するフィルタ
ゲイン算出手段１３と、地磁気センサの出力から求めた
方位データθ”、および旋回角速度センサの出力から求
めた方位データθ６に、上記カルマンフィルタゲインＫ
に基づいた重み付け処理をすることにより、移動体の現
在の推定方位θを求める方位推定手段１１とを有するも
のである。

なお、ノイズ算出手段が地磁気センサの出力データθＨ
に含まれるノイズ成分σ１、および旋回角速度センサの
出力データΔθＧに含まれるノイズ成分σＧを、走行デ
ータに基づいて算出するものであってもよい。

く作用〉 地磁気センサの方位データθＨ１および旋回角速度セン
サの出力から求められる方位データθＧは、それぞれラ
ンダムノイズ、オフセットを含めた形で次のように表わ
される。

θＨ−θ＋Ｖ θＧ−〇＋Δθ’＋Ｗ＋ｏ（ｔ） ここに、ｖ、Ｗは、地磁気センサ、旋回角速度センサの
出力に含まれるランダムノイズであり、それらの平均値
はＯである。ｏ　（ｔ）はオフセット値であり、時間と
ともに変化する。

本発明の方位検出装置によれば、ノイズ算出手段１２に
より、上記ランダムノイズ成分ｖ、Ｗとして、地磁気セ
ンサの出力データθＨ１旋回角速度センサの出力データ
ΔθＧのノイズσＨ１σＧを記憶しておき、オフセット
０（ｔ）を、移動体停止時の実測オフセット値と、停止
から次の停止までの期間とに基づき具体化する。

すなわち、オフセット値は移動体が停止したときに実測
することができるので、上記関数□（１）は１−０で一
定値Ａ１とおける。また、停止から次の停止までの期間
をＴとし、今回の停止時の実測オフセット値をＡ２とす
るとｏ　（Ｔ）−Ａ２とおける。このことは、関数０（
ｔ）に境界条件を与えることを意味する。関数形０（ｔ
）を理論または実測により仮定できれば、これにより、
実際に用いられている旋回角速度センサのオフセット特
性を正確に反映するオフセット関数０（ｔ）を求めるこ
とができる。

そして、式（σＧ＋ｏ（ｔ））２により見掛けのノイズ
成分を計算する。これにより、各センサの出力データθ
Ｈの信頼性とともに、ΔθＧの信頼性をオフセット分を
含めて評価することができる。沖！定されたノイズの値
は、フィルタゲイン算出手段１３に供給され、カルマン
フィルタゲインＫが算出される。

そして、カルマンフィルタ手段１１において、所定の重
み付け処理が施され推定方位θが出力される。

このようにして、ΔθＧについて、オフセット値の変化
に応じた重み付けを行うことができるので、旋回角速度
センサのオフセット特性に応じた、正確な移動体の方位
を推定することができる。

また、ノイズ算出手段がノイズ成分σＨσＧを走行デー
タに基づき収集するものであれば、実際の走行条件に応
じたσ８　σ６の評価をリアルタイムで行うことができ
、推定方位の信頼性をさらに向上させることができる。

なお、上記走行中のノイズ成分を測定する理由は、走行
中の振動や、移動体の動特性等に起因するノイズを含ん
でいる走行中のデータによってノイズを評価することが
好ましいからである。

特に、旋回による角速度成分を含まない直線走行中のデ
ータを利用することがより好ましい。

〈実施例〉 以下実施例を示す添付図面によって詳細に説明する。

第３図は、車両に適用された本発明の方位検出装置の一
実施例を示すブロック図であり、・左右両輪の回転数を
検出する車速センサ４１（このセンサは、距離センサと
して利用される。）・地磁気センサ４２、 ・ジャイロ４３（旋回角速度を干渉光の位相変化として
読み取る光フアイバジャイロ、ピエゾエレクトリック素
子の片持ちぼり振動技術を利用して旋回角速度を検出す
る振動ジャイロ、機械式ジャイロ等から選ばれたもの。

旋回角速度センサとして利用される。）、 ・道路地図データを格納した道路地図メモリ２、・ジャ
イロ４３、地磁気センサ４２により検出された出力デー
タに基づいて車両の推定方位を算出するとともに、車速
センサ４１のデータと合わせて車両の位置出力データを
演算しバッファメモリ３に格納するロケータ１、 ・当該読出した車両現在位置を地図に重ねてデイスプレ
ィ７に表示させるとともに、キーボード８とのインター
フェイスをとるナビゲーションコントローラ５ とから構成されている。

上記ロケータ１は、例えば、車速センサ４１からの出力
パルス信号の数をカウンタでカウントするこ六により車
輪の回転数を得、カウンタから出力されるカウント出力
データに対して、乗算器により１カウント当りの距離を
示す所定の定数を乗算することにより単位時間当りの走
行距離出力データを算出するとともに、ジャイロ４３か
ら車両方位の相対変化を求め、これと地磁気センサ４２
の絶対方位出力データとから車両の方位出力データを算
出するものである。

上記道路地図メモリ２は、所定範囲にわたる道路地図デ
ータが予め格納されているものであり、半導体メモリ、
カセットテープ、ＣＤ−ＲＯＭ。

ＩＣメモリ、ＤＡＴ等が使用可能である。

上記デイスプレィ７はＣＲＴ、液晶表示器等を使用して
、車両走行中の道路地図と車両位置とを表示するもので
ある。

上記ナビゲーション・コントローラ５は、図形処理プロ
セッサ、画像処理メモリ等から構成され、デイスプレィ
７上における地図の検索、縮尺切り替え、スクロール、
車両の現在位置の表示等を行わせる。

上記の構成の装置による車両方位検出手順について説明
する。車両走行中は、上記ロケータ１に取り込んだ各セ
ンサ出力データに基づいて、車両の位置をデイスプレィ
７上に地図とともに表示しているが、その表示中も一定
時間ごとの割り込みにより各センサの出力データを取り
込み、車両方位を更新するようにしている。この割り込
み時の車両方位検出フローを第２図に示す。なお、この
割り込みは、車両の走行距離出力データを基にして求ま
る一定の走行距離ごとに行ってもよい。上記一定時間ま
たは一定走行距離の長さは、使用されるジャイロの種類
や地磁気センサの性能等により適宜設定される。

まず、ステップ■において、ジャイロ４３の出カデータ
ΔθＧと地磁気センサ４２の出力データをθ８を取り込
む。

次に、車両が直線走行中かどうか判定する（ステップ■
）。直線道路を走行中かどうかは、比較的短期間のジャ
イロ４３の出力データに基づいて簡単な比較計算をすれ
ば十分判断可能である。すなわちジャイロ４３の出力デ
ータΔθＧと前回の推定方位θとを用いて、 θ６−θ＋ΔθＧ を計算する。この値と、例えば過去何回かの推定方位の
平均値くθ〉との差１θ６−くθ〉１が基準値ｅより少
なければ直線走行中と判断できる。

このほかに、マツプマツチング手法により判断してもよ
い。

直線走行中でなければ、方位検出に利用するため、上記
推定方位θＧを登録して（ステップ■）、割り込み継続
回数を示すカウンタｎを０とおき（ステップ■）、ステ
ップ［株］に進む。

直線走行中であれば、カウンタｎを１だけインクリメン
トしくステップ■）、センサ出力データΔθＧ　θ”を
バッファメモリ３の第ｎ番地に記憶しくステップ■；以
下、第ｎ番地に記憶されたデータをΔθＧ　ｎ１θＨｎ
で表わす）、ｎがバッファメモリ３の容量等から決まる
最大値ｍに達しているかどうか判定する（ステップ■）
。最大値ｍに達していれば、以下の計算をするのに十分
な数のデータが集められたと判断して、ステップ■以下
に進む。最大値ｍに達していなければ、割り込み前の状
態に戻り、上記の処理を繰り返す。

ステップ■では、過去ｍ回で集められたデータΔθ’　
ｎ　（ｎ−ト１）を用いて平均値、−１ 〈Δθ’　　＞　＝　（１／ｍ）　　Σ　ΔθＧ　　ｎ
　　。

ｎ雪１ 分散値、　　　　。、。

σＧ　　２　−　（１／ａｔ）　　Σ　　（ΔθＧ　ｎ
−〈θＧ〉）２ｒｉ を求める。

ステップ■では、過去ｍ回で集められたデータθ’　ｎ
　（ｎ−ト１）を用いて平均値、ｎ−ｍ 〈θ’　　＞　−（１／ａ＋）　　Σ　θＨｎｒ１ 分散値、 −ｍ σ”　＝　（１／＋）　　Σ　（θＨｎ　　＜θＨ〉）
２ｎ＝１ を求める。

上記のようにして求めた分散値σＧ２　σＨ２はセンサ
の出力データの、正規分布に従うランダムノイズの大き
さに対応するものである。

次にジャイロ４３の出力データΔθＧのオフセットを求
める。第４図はジャイロ４３の出力データの瞬時値を表
わすグラフであり、停車時ｔ２から次の発進時ｔ３まで
の間にオフセット値がＡからＯに補正され、発進後再び
オフセットｏ　（ｔ）が現れ、時間ｔとともに増加して
いく様子を示している。もし、オフセットの関数形ｏ　
（ｔ）を時間の一次関数αｔと仮定すれば、オフセット
補正値Ａを前の発進時ｔｌから停車時ｔ２までに要した
時間Ｔ−ｔ２−ｔｌで割ることによりオフセットの時間
変化率αを求めることができる。

上記時間変化率αは、停車、発進を繰り返すごとに求ま
るので、その都度αを更新していけばよい。これによっ
て、温度等によるオフセットのドリフト分を各時点に応
じて正確に評価することができる。

上記のように関数形ｏ　（ｔ）を時間の一次関数αｔと
仮定したのは、オフセット値が第４図に示すように、時
間とともに増加することが多いので一次関数で十分近似
できると考えたからであるが、−次間数に限られるもの
でなく、旋回角速度センサの特性や実測データに応じて
最適な関数を決定すればよい。例えば、ｏ　（ｔ）の形
として、二次関数αｔ２、指数関数α（ｅ’−１）等を
仮定してもよい。

そこで、前回求めたオフセットの時間変化率αを用いて
、次式 ％式％） により見掛けの分散値を計算する。

なおこの実施例では、データΔθＧのサンプリングは時
間Δｔごととしており、ステップ［株］において、Δｔ
ごとに増加するオフセット増加率α′−α／Δｔを用い
て （σ６＋α′ｎ）２ で計算している。以後この見掛けの分散値をジャイロ４
３の出力データΔθＧの分散値と見なすことにし、改め
てσＧ２とおく。

ステップ■では分散値σ６２．σＨ２を考慮にいれた推
定方位を次式に基づいて計算する。

θｉ　−（１−Ｋｉ　）　　（θｉ−１＋Δθ’ｉ）＋
Ｋｉ　θＨ１ ここに、θｉは今回の割り込みで求める推定方位、θｉ
−１は前回に求めた方位である。ΔθＧｉ。

θＨｉは今回推定方位を計算するときに使用するセンサ
の出力データであり、例えば最新のデータΔθＧ　ｍ、
θＨｒｔ、を使用してもよいし上記平均値くΔθ’＞＜
θ”〉を使用してもよい。Ｋ１は０＜Ｋｉ＜１なる変数
であって、カルマンゲインと呼ばれる。Ｋｌは前回に求
めたカルマンゲインＫ　ｉ−１を使って、 σＧ　２　＋σＧ２＋Ｋ１−１　　σＨ２として求めら
れる。

以上のようにして、特にジャイロ４３のオフセットを考
慮に入れたノイズ量に応じてカルマンゲインＫｉをリア
ルタイムで求め、重み付けをした結果として、新しい車
両の方位θｌを求め（ステップ＠）、初期状態に戻る（
ステップ＠）。

この方位θｌと車速センサ４１の距離データとから車両
の推定位置を算出することができる。

勿論この時に道路地図データと比較し、道路地図データ
との相関度を評価して車両の推定位置を補正し、車両の
現在位置を道路上に設定するマツプマツチング方式を採
用してもよい（特開昭６３−１４８１１５号公報参照）
。

以上、実施例に基づいて本発明の方位検出装置を説明し
てきたが、本発明は上記実施例に限るものではない。例
えば、上記実施例のステップ■■では、過去の走行で集
められたデータΔθＧ　ｎ。

θＨｎを用いて平均値、分散値を求めていたが、旋回角
速度センサ、地磁気センサの特性に応じた一定値を予め
記憶しておき、その値のみを用いて計算してもよい。こ
うすることによって、制御が簡単になるとともに、直線
走行中でない場合でも正確な方位を推定できるようにな
る。

また、車両の代わりに航空機、船舶等の移動体に適用す
ることができ、その他、本発明の要旨を変更しない範囲
内において、種々の設計変更を施すことが可能である。

〈発明の効果〉 以上のように、本発明の方位検出装置によれば、旋回角
速度センサのオフセット値を、移動体停止時の実測オフ
セット値と、停止時から停止時までに計測した期間とに
基づき、時間とともに変化する関数ｏ　（ｔ）として予
測し、地磁気センサの出力データθＨおよび旋回角速度
センサの出力データΔθＧに含まれるノイズ成分と、上
記予測されたオフセットｏ　（ｔ）とを用いて、旋回角
速度センサの実際のオフセット特性を反映した移動体の
正確な方位を推定することができ、上記方位に基づいて
移動体の移動中の位置を正確に求めることができる。

また、地磁気センサの出力データθＨおよび旋回角速度
センサの出力データΔθＧに含まれるノイズ成分を実走
行データに基づきリアルタイムで求めることにすれば、
より正確な方位を推定することができる。

【図面の簡単な説明】

第１図は本発明の方位検出装置の機能ブロック図、 第２図は方位検出手順を示すフローチャート、第３図は
方位検出装置のハードウェア構成を示すブロック図、 第４図は旋回角速度センサのオフセット値の時間変化を
示すグラフである。 １・・・ロケータ、３・・・バッファメモリ、１１・・
・方位推定手段、１２・・・ノイズ算出手段、１３・・
・フィルタゲイン算出手段、 １ ５・・・オフセラ ト予測手段、 ２・・・地磁気センサ、 ３・・・ジャイロ

Claims (1)

1. 【特許請求の範囲】 １、地磁気センサの方位データ、および旋回角速度セン
   サの出力から求められる方位データを取り込み、それら
   の値と過去の推定方位とから移動体の現在の推定方位を
   求める方位検出装置において、 旋回角速度センサのオフセット値を、移動 体停止時の実測オフセット値と、停止時から次の停止時
   までに計測した期間とに基づき、時間の関数ｏ（ｔ）と
   して予測するオフセット値予測手段と、 地磁気センサの出力データθ＾Ｈのノイズ成分σ＾Ｈを
   記憶するとともに、旋回角速度センサの出力データΔθ
   ＾Ｇのノイズ成分σ＾Ｇを記憶し、上記予測されたオフ
   セットｏ（ｔ）を用いて旋回角速度センサの出力データ
   Δθ＾Ｇの見掛けのノイズ成分を次式により計算するノ
   イズ算出手段と、 （σ＾Ｇ＋ｏ（ｔ））＾２ 上記ノイズ成分を用いてカルマンフィルタ ゲインＫを算出するフィルタゲイン算出手段と、 地磁気センサの出力から求めた方位データ θ＾Ｈ、および旋回角速度センサの出力から求めた方位
   データθ＾Ｇに、上記カルマンフィルタゲインＫに基づ
   いた重み付け処理をすることにより、移動体の現在の推
   定方位θを求める方位推定手段とを有することを特徴と
   する方位検出装置。 ２、ノイズ算出手段が、地磁気センサの出力データθ＾
   Ｈに含まれるノイズ成分σ＾Ｈ、および旋回角速度セン
   サの出力データΔθ＾Ｇに含まれるノイズ成分σ＾Ｇを
   実際の走行データに基づいて算出するものである請求項
   １記載の方位検出装置。