JPH03188317A - 方位検出装置 - Google Patents

Abstract

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

Description

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

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

Δｙを加算することにより、現在の位置出力データ（Ｐ
ｘ、Ｐｙ）を求める方式であるが、方位センサが必然的
に有している誤差のために、得られる現在位置データに
含まれる誤差が累積されてしまうという欠点がある。

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

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

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

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

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

しかもこのオフセット出力は、旋回角速度センサ出力デ
ータのランダムノイズとして検出することができない。

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

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

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

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

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

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

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

本発明の方位検出装置によれば、ノイズ算出手段１２に
より、上記ランダムノイズ成分ｖ、Ｗとして、地磁気セ
ンサの出力データθ”、旋回角速度センサの出力データ
ΔθＧのノイズσ１．σＨを記ｔａしておき、オフセッ
ト値ｏ　（ｔ）の関数形を理論もしくは実測により仮定
し、式 ４式％（）） により見掛けのノイズ成分を計算する。

これにより、各センサの出力データθＧの信頼性ととも
に、ΔθＧの信頼性をオフセット分を含めて評価するこ
とができる。測定されたノイズの値は、フィルタゲイン
算出手段１３に供給され、カルマンフィルタゲインＫが
算出される。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

次に、ジャイロ４３の出力データΔθＧのオフセットを
求める。第４図はジャイロ４３の出力データの瞬時値を
表わすグラフであり、停車時ｔ２から次の発進時ｔ３ま
での間にオフセット値がＡからＯに補正され、発進後再
びオフセットｏ　（ｔ）が現れ、時間ｔとともに増加し
ていく様子を示している。したがって、発進時ｔ３以後
のオフセットの関数形をｏ　（０）−０となる適当な形
に仮定し、次式 ％式％（）） により見掛けの分散値を計算する。

ｏ　（ｔ）の形は、ジャイロの特性に応じて理論的また
は実験的に決定すればよい。例えば、時間とともに増加
する一次関数αｔ、二次関数αｔ２指数関数α（ｅ’−
１）等である。

以後この見掛けの分散値をジャイロ４３の出力データΔ
θＧの分散値と見なすことにし、改めてσＧ２とおく。

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

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

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

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

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

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

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

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

く計算例〉 本発明の有効性を確認するために、次のシミュレーショ
ンを行った。

地磁気センサの出力がθＨ１旋回角速度センサの出力が
、 ΔθＧｎ＝ΔθＧＯ＋βｔ　　■ （ΔθＧ０は初期値、βは定数）の形の疑似入力を作り
、簡単のため、方位変化がない（直線走行中）と仮定し
た。したがってΔθ’ｏ−ｏ、　　θＨ−一定値とおい
た。また、データの分散値σ′（２も一定値とおいた。

そして、ｏ　（ｔ）を−次間数αｔと仮定し、αの値を
種々変えながら、見掛けの分散値を計算した。

（１）まず、α−０の場合、すなわちオフセットの変化
を考慮に入れないで式 ％式％） により推定方位を求めた。この場合カルマン定数Ｋｉは
σ１．σＨで決まる定数となる。

以上のように求めた推定方位と、上記０式の疑似入力に
従う「真の」方位との誤差が蓄積されていく傾向をグラ
フ化したのが第５図（ａ）である。

いずれも、オフセットにより誤差が線形に増大していく
のが判る。特に、σＧが小さな場合は地磁気センサのデ
ータよりも旋回角速度センサのデータに重きを置いてい
るので、誤差の累積が顕著である。

（２）次にα≠０、すなわちオフセットを考慮に入れて
計算を行った。σＨを一定とおき、αを３種類設定して
、誤差が蓄積されていく傾向をグラフ化したのが第５図
（ｂ）である。いずれも、誤差が一定の値に収束してい
ることか判る。特に、αが大きければ、地磁気センサの
データに重きがおかれる結果、蓄積誤差は速く収束し、
αが小さければ、旋回角速度センサのデータに重きがお
かれる結果、蓄積誤差は増大しゆっくり収束する。

以上のように、オフセットを考慮にいれた計算により、
誤差が一定値以内に収まることが判った。

これにより、本発明の効果が多大なものであることが立
証できた。

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

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

【図面の簡単な説明】

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

Claims (1)

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