JPH03282212A - 移動体航法装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔産業上の利用分野〕 この発明は自動車のような移動体の位置、方位を求め、
道路地図パターンなどと重ねて表示器に表示し、目的地
までの効率の良い走行を支援するような移動体航法装置
に関するものである。

〔従来の技術〕

従来、この種の移動体航法装置としては、地ｉ気方位セ
ンサを用いて移動体の方位を検出し、速度センサを用い
て移動体の速度を検出し、移動体の微小時間当りの移動
距離と前述の方位とから成る移動ベクトルをスタート位
置に積算して、遂次移動体の位置を求めるものかあフた
。しかしながら、地磁気センサで得られる方位テークか
一般的に道路環境によって乱れていたり、偏っていたり
するために得られる位置の正確さはきわめて低かった。

そこで、例えば、角速度（ヨーレート）を求める角速度
センサを併設する考え方が登場した。

そのようなもので、例えば地磁気方位データが乱れた場
合は角速度センサデータを移動体の方位とするものかあ
った（特開昭５７−８４３１０号公報、特開昭５８−３
３２８３号公報）。これでは地磁気が連続的にみたれて
いる場合、角速度センサデータに継続的に依存すること
となり、角速度センサの誤差によりやがて誤った方位を
求めてしまう問題があった。

また、一定時間毎または停止毎に地磁気方位センサによ
る方位変化と比較してレート式ジャイロスコープの出力
のドリフト誤差（時間当りの角度誤差）を算出し、相当
分を以後の出力から減じて補正するものかあった（特開
昭５８−３４４８３号公報、特開昭５９−２１８９１４
号公報）。

また、この角速度センサによる角度変化と地磁気方位セ
ンサによる方位変化を比較して角速度センサのドリフト
誤差を求め、この誤差に応じた補正係数（ノンストップ
走行時間の関数）で重み付けした上で合成して（角速度
センサ方位を補正して）移動体方位とするものがあった
（特開昭５９−２１８９１３号公報）。

ところか、これらは、いづれも時間と共に角度出力が変
化するようなドリフト誤差に対処するものであったため
に、そのような性質ではなく移動体の移動に伴って誤差
が累積するような性質を有する車輪速センサによる角速
度センサを使用する場合においては、当該ドリフト誤差
分を補正しても何等効果が得られないばかりか、外乱を
ドリフト誤差と誤って検出し、むしろ補正ではなく改悪
されるか、あるいは誤差の振動を発生するどいフた問題
かあった。

言い替えれば、これらの従来例のものでは、異なる性質
の誤差を有する角速度センサデータの補正に関する概念
（手段、作用の例なと）は開示されていなかった。

一方、時間と共に出力がドリフトするような誤差はほと
んどなく、スリップ制御の目的で設置された車輪回転セ
ンサか流用できる格別のセンサが不要のいわゆる車輪速
センサによって角速度を求めるものか１主目されたした
。

これは、車輪の回転速度を左右それぞれ個別に検出して
その差を左右車輪間距離で除して移動体の角速度（ヨー
レート）を求めるような角速度センサである。

ところか、この角速度センサはタイヤの摩耗、空気圧の
変化、道路状態の変化などにより、レートジャイロなど
とは異なる要因の誤差、つまり移動につれて累積するよ
うな角度誤差を生じてしまうといった問題点を有してい
た。

この問題を解決しようとしたものに、左右車輪それぞれ
の回転数に対する移動体の移動距離を示す回転数−移動
距離変換係数を左右車輪速センサデータに基ついて補正
するものがあった（特開平１−９８９２１号公報）。こ
れは、タイヤ径の変動などによる回転数あたりの移動距
離のドリフトなどに対して補正をするものである。とこ
ろが、これにおいては、各データが本質的に誤差あるい
は外乱成分を有することから、これらが補正結果へ直接
的かつ鋭敏に反映されてしまい、外乱の入った場合とそ
の直後に大きく誤った結果を出してしまうという致命的
な問題点を有していた。

（発明か解決しようとする課題） 以上のような従来の移動体航法装置では外乱の大きい場
合や素データの検出・演算誤差がある場合、つまり通常
の場合においてくろいのある方位、ひいては誤った位置
か得られてしまうという問題点かあった。

つまり、精度のよい位置を検出するためには、方位、距
離（速度・時間）をより正確に検出しなければならない
という課題が依然として残っていた。

この発明は上記のような問題点を解決するためになされ
てもので、絶対方位データおよび角速度データに、外乱
成分か含まれていても、（検出・演算誤差か含まれてい
ても）これらの影響か補正結果にほとんど反映されない
で、角速度を補正し、結果的に大幅に精度が高く、ひい
ては精度の高い移動体位置の検出が可能な移動体航法装
置を提供することを目的とする。

（３題を解決するための手段〕 この発明の移動体航法装置は、移動体の絶対的な方位Ω
を検出する絶対方位検出手段、移動体の角速度δを検出
する角速度検出手段、移動体の速度Ｖを検出する速度検
出手段、これらで検出されたデータに基づいて角速度を
補正する角速度補正手段、補正された角速度δ、方位Ω
、速度Ｖに基づいて移動体方位！を演算する方位演算手
段、移動体方位！および速度Ｖに基づいて移動体の位置
を演算する位置演算手段を備えた移動体航法装置であっ
て、上記角速度補正手段は、所定距離以上の移動に伴な
い、それまでの角速度補正係数で補正された角速度によ
る方位変化と絶対方位による方位変化から、移動距離あ
たりの方位変化誤差Ｇを演算し、この誤差に応じて、所
定の割合だけ、以後の角速度補正係数を補正するように
したものである。

〔作用〕

この発明における角速度補正手段では、所定の移動距離
に伴なう、絶対方位検出手段の検出データに基つく方位
変化と、それまでの所定の補正をした後、角速度に基つ
く方位変化から、移動距離あたりの方位変化誤差Ｇ　（
ｒａｄ／ｍ　）が演算される。

この値は、その時々の速度を乗ずればその時々の角速度
誤差を与えるようなものである。

そのような値を減少する極性で、かつ誤差の角速度換算
値の所定の割合（学習係数・誤差）が以後の角速度変換
係数の補正値として与えられるように演算され、角速度
補正係数が補正されて、以後の移動において、その時々
の速度、係る角速度補正係数に基づいて角速度検出手段
での検出データが算出される。

角速度変換係数の初期値は移動体固有の計測値あるいは
［０］が与えられ、以後、この処理か縁り返し実行され
、道路状況などが変化なければ、徐々にある値に集束し
ていく、長期間の走行で道路状況やタイヤ径、空気圧の
変化などが起こった場合、その状況に適応じて、角速度
補正係数は異なる値へ集束していき、角速度センサデー
タに外乱が入った場合も、その影響はほとんど入り込ま
ない。

〔実施例〕

以下、この発明による移動体航法装置について述べる。

第１図はその一実施例を示すブロック図である。

この発明は、道路状態、移動体運行状態などが変化せず
に、ある所定の距離を越えて走行すると越えれば越える
だけ、角速度を積算して得られる方位変化は絶対方位セ
ンサデータで示される方位変化よりも累積誤差か増大す
るような車輪速センサの性質（第２図に一例を示す）、
方位変化の真値からの誤差が両方のセンサデータの差で
表現される点、誤差を生じた原因と考えられる角速度補
正係数の誤差相当分を１００パーセント補正するのでは
なく、所定の割合η（０くη〈１）たけ補正すると、道
路状態、移動体運行状態などが変化した場合、絶対方位
Ωに外乱が入った場合、角速度δに外乱が入った場合な
どにおいても、それらの悪影響か角速度の補正結果にほ
とんど出てこない点に着目してなされたものである。

第１図において、（１）はこの発明の特徴的部分である
角速度補正手段、（２）は角速度検出手段、（３）は絶
対方位検出手段、（４）は速度検出手段、（５）は方位
演算手段、（６）は位置演算手段である。（１１）は角
速度補正演算部、（１２）は相対方位変化演算部、（１
３）は移動距離演算部、（１４）は推定誤差演算部、（
１５）は補正係数演算部である。

このような構成の移動体航法装置において、角速度検出
手段（２）は、例えば左右車輪の回転速度を示すパルス
周波数ＦＲ，ＦＬを検出し、δ゛＝ＫＯ・（ＦＲ−ＦＬ
）／Ｔ　（但し、ＫＯ，定数）のような演算をして、移
動体の旋回の角速度（ヨーレート）に対応する角速度δ
′を求める。

絶対方位検出手段（３）は、例えば検出部か移動体に固
定された地磁気方位センサであり、その場合互いに直交
する二つの方位の地磁気強度Ｕ。

■を検出し、その位置固有の偏角データθを用いて、 Ω＝　ａｒｃｔａｎ　（Ｖ　／　Ｕ　）　　−〇のよう
な演算をして、移動体の地図上の方位Ωを求める。

速度検出手段（４）は、プロペラシャフトの回転角に比
例して発生するパルスの周波数Ｆを検出し、 Ｖ＝Ｋｌ　−Ｆ　（但し、Ｋｌ　；定数）のような演算
をして、移動体の速度Ｖを検出する。

この発明の特徴的部分である角速度補正手段（１）は、
角速度δ′、方位Ω、速度Ｖに基づいて、これらのデー
タの傾向・性質に適応しながら、角速度δを補正し、後
述の方位演算手段（５）へ供給する。

この角速度補正手段（１）において、角速度補正演算部
（１１）は、それまでの角速度補正係数Ｋｉおよび速度
Ｖに基づいて δ＝δ’　＋Ｋｉ　　・■ のような角速度補正の演算をして、後述の方位演算手段
へ供給するとともに、相対方位変化演算部（１２）へ供
給する。

移動路ｍ演算部（１３）は、 のように、速度■を所定の距離ＬＭを越えるまで積算し
て、移動体の移動距離りを求め、また、その区間のタイ
ミング（ｔｉ〜ｔｉ−１）を与える。

相対方位変化演算部（１２）は、 のように、補正済みの角速度δを移動距離りの積算時間
に対応する期間（ｔｉｘｔｉ−１）にわたって積分して
、相対方位変化角Φを求める。

誤差推定演算部（１４）は、タイミング（ｔｉ”ｔｉ−
１）における絶対方位Ωｉ、Ωｉ−１の差ω（＝Ω１−
Ωｉ−１　）　、および相対方位変化角Φおよび移動路
ｌＩＬに基づいて、 Ｇ＝（Φ−ω）／Ｌ のような演算をして、絶対方位変化ωを基準（実際には
誤差、外乱を含む）にした相対方位変化Φの移動距離あ
たりの方位変化誤差Ｇを求める。

補正係数演算部（１５）は、それまでの角速度補正係数
Ｋｉおよび所定の学習係数η（ηはｏくηく１、望まし
くは１７１０００〜０．５）、および推定誤差Ｇに基つ
いて、 Ｋｉ＋１＝Ｋｉ−η・Ｇ の演算をして、新たな角速度補正係数にｉ＋１を求める
。

方位演算手段（５）は、例えば速度Ｖに基づいて距離を
求め、所定距離ごとに補正された角速度δ、絶対方位Ω
に基づいて、移動体方位！を演算し、位置演算手段（６
）へ供給する。

なお、時間ｔｊにおける移動体方位！ｊは平Ｊ　＝！ｊ
−１＋（ΩＪ−＋−Ωｊ）・Ｐ＋Φｊ・（１−Ｐ）で計
算される。

ここで！ｊ−１は全開の移動体方位、Ωｊ−１，Ωｊは
時間ｊｊ−＋、　ｊｊにおける絶対方位、Ｐは定数、位
置演算手段（６）は、遂−５速度Ｖから、ｄｉ−Ｖ−ｄ
ｔ ように演算して、微小移動距離ｄｉを求め、これと移動
体方位型に基ついて、 Ｘ　ニーＸ　１−Ｄｄｌｃｏｓ平 Ｙ　ｉ−Ｙ　ｉ−１＋ｄｉｓｉｎ’Ｐ の様な演算をし、位置Ｘｉ、Ｙｉを求め、表示手段（図
示を省略）により必要に応じてその位置に対応する領域
の地図を描き、また移動体方位！の方位を向いた移動体
のマークをその位置に描く。

上記の実施例で示した装置は、例えば第３図に示すよう
なコンピュータヘースの装置を用いて、データ処理の大
部分を演算処理によフて実現してもよい。

第３図中、（３００）はＣＰＵ、（３２１）は初期位置
、角速度補正係数などを装置の停止時に待避（バックア
ップ）しておく不揮発性メモリ（ＲＡＭ）　　　（３１
０）はプログラムメモリ（ＲＯＭ）、（３２０）は演算
の途中結果などを記憶するＲＡＭ、（３３０）は地磁気
方位センサ（３９０）で検出され、Ａ／Ｄ変換器（３７
０）、（３８０）でＡ／Ｄ変換された地磁気強度データ
Ｕ、■を入力するインターフェース（ＰＩＡ）、（３５
０）は、車速センサ（４００）で検出された速度パルス
を、システムタイマ（ＰＴＭ）　　（３４０）に基づく
一定のインターバル時間について順次カウントして、速
度■を求めるためのイベントカウンタ（ＰＴＭ）、（３
５１）、（３５２）は、同様にして、左右後車輪の回転
速度を求めるためのイベントカウンタ（ＰＴＭ）、（５
００）は、他の方位補正手段のための例としての人口衛
星からの航法電波受信器（５２０）、受信信号処理器（
ＧＰＳＲ）（５１０）からの方位データなどを入力する
インターフェース（ＳＣＩ）、（３６０）は、図示して
ないＣＤ−ＲＯＭおよびそのドライバに記憶された地図
データ、演算により求めた移動体の位置、方位に基づい
て画像メモリ（ＤＲＡＭ）（３６１）に画像を描き、そ
のデータを映像信号変換器（ＰＳＣ）　　（３６２）を
介して、表示器（ＣＲＴ）（３６３）に供給するなとの
制御を行う表示コントローラ（ＣＲＴＣ）である。

（３３１）は、操作器（ＫＥＹ）　　（３３２）を介し
て入力される初期位置の人力、移動体位置の修正などの
操作情報を人力するインターフェース（Ｐ　Ｉ　Ａ）で
ある。

ブロクラムメモリ（３１０）には、上記実施例に対応す
る処理で、例えば、次に示すような処理アルゴリズムの
ブロクラムかコート化されて書き込まれている。

初期化処理として、 １、コールドスタートならば、前回の車輪角速度補正係
数Ｋｉを初期化する。また、車両位置Ｘｉ、Ｙｉを人力
し、前回の車両位置Ｘ　ｉ−１、Ｙｉ−１およびバック
アップ位置Ｘｂｐ、　Ｙｂｐを車両位置Ｘｉ、Ｙｉて置
換える。

即ち、Ｋｉ←ｏ、Ｘｌ−１４−Ｘｉ　、　　Ｙｉ−１←
Ｙｉ　、ｘｂｐ←ｘｉ１Ｙｂｐ＋−Ｙｉ ホットスタートならば、前回の車輪角速度補正係数Ｋｉ
をバックアップ係数Ｋｂｐで、前回の車両位置Ｘ１−１
　、　Ｙｉ−１をハラ’７７　ｙブ位置Ｘｂｐ、Ｙｂｐ
で置換える。

即ち、ｌ（ｉ←にｂｐ、　　Ｘｌ−１−Ｘｂｐ、Ｙｉ−
１←Ｙｂｐ２、Ｎｌ算移動距離し、積算方位変化Φを初
期化する。

即ち、Ｌ←０、Φ←０ ３、絶対方位Ωを入力し、その値で前回の絶対方位Ωｉ
−１を置き換える。

即ち、Ωｉ−１←Ω ４、リターンする。

定時（Δｔ）毎の処理として、 １、車速Ｖを検出する。

２、車速Ｖから微小移動距離ｄｉを演算する。

即ち、ｄｌ＝Ｖ・Δｔ ３、微小移動距離ｄｉを積算する。

即ち、Ｌ−Ｌ＋ｄｌ（Ｌを移動距離と呼ぶ。）４、角速
度δ゛を検出する。

５、車輪角速度補正係数Ｋｉをパラメータとして、速度
Ｖに基づいて、角速度δ°を補正する。

即ち、δ＝δ’　　＋Ｋｉ　　・■ ６、補正済みの角速度δを積算する。

即ち、Φ←Φ＋δ（Φを相対方位変化と呼ぶ。）７、絶
対方位Ωを人力する。

８、車両方位！を演算する。

即ち、！＝ｆ（Ω、δ、■、・・） ９、車両位置Ｘ　ｉ、Ｙ　ｉを演算する。

即ち、Ｘｉ　＝Ｘｉ−１＋ｌ　　・ｃｏｓψＹｉ　＝Ｙ
ｉ−１＋１　−５ｉｎ　’！１０、積算移動距離りか所
定距離Ｌｍｉｎに満たなければ、１７．の処理ヘシャン
プする。

１１、絶対方位変化ωを演算する。

即ち、ω＝Ω−Ω１−１ １２、絶対方位変化ωの絶対値か所定値を越えているな
らば、１８．の処理ヘジャンブする。

１３、前回の絶対方位Ωｉ−１を今回の絶対方位Ωて置
き換える。

即ち、Ωｉ−１←Ω １４、推定誤差Ｇを演算する。

即ち、Ｇ＝（Φ−ω）／Ｌ １５．車輪角速度補正係数にｉを演算・学習する。

即ち、Ｋｉ＋１＝Ｋｉ−η・Ｇ　（η；学習係数（−〇
〜１）） １６、車輪角速度補正係数にｉとバックアップ係数にｂ
ｐを更新する。

即ち、Ｋｉ←にｉ＋１　、　　Ｋ　ｂｐ４−Ｋ　ｉ＋１
１７、積算移動路１１Ｌ、積算方位変化Φをクリアする
。

即ち、Ｌ←０、Φ←０ １８、車両位置Ｘｉ　、Ｙｉを補正／修正する。

１９、前回の車両位置Ｘ１−１　、Ｙｉ−１およびノル
ツタフップ位置ｘｂｐ、　Ｙｂｐを更新する。

即ち、Ｘｌ−１４−Ｘｉ　、　Ｙｉ−１４−Ｙｉ　、　
　Ｘｂｐ＋Ｘｉ　、　Ｙｂｐ４−Ｙｉ ２０、車両位置）（ｉ　、Ｙｉへその領域の地図、車両
方位マークを適時描画する。

２１、リターンする。

（なお、コールドスタートかホットスタートかの判定は
不揮発性メモリへ、例えば装置の起動の度に、所定のコ
ードデータを書き込むようにしておき、起動時にそのデ
ータを読み、所定の値であればホットスタート、そうで
なければコールドスタートと判定する。） 以上の実施例では、地磁気方位センサデータに基づいた
方位変化角ωを基準とした相対方位変化データの誤差Ｇ
を抽出し、それに基づいて新たな角速度補正係数を算出
したが、上述の移動体方位！、あるいは他の手段による
補正がさらになされた移動体方位、例えば、得られた軌
跡をデジタル道路地図データベースの道路形状と照合し
て一致度（相関）がよいときのデジタルデータベースの
対応する道路リンクの方位に基づく方位変化角を基準に
して、誤差を演算するようにしてもよい。

また、絶対方位検出手段は地磁気方位センサによるもの
に限定されるものではなく、移動体の移動中に複数の人
工衛星からの航法電波を受信し、演算して得られる移動
体の方位、地上の各所に配置された送信源から無線で供
給される道路の方向（方位）データなとであってもよい
。

これらの場合、地磁気方位より正確な絶対方位が得られ
る場合、より正確でかつ早い角速度の補正か達成できる
。

また、所定の移動距離ＬＭは、上記実施例では定数とし
て述べたが、角速度補正係数の学習の進展度に応じて変
更するようにしてもよい。この場合、残留偏差が小さく
出来るメリットがある。

また、地磁気方位センサを用いる場合において、絶対方
位変化が所定の値以下の場合に限って、角速度補正係数
を学習するようにしてもよい。この場合、移動路１ｉ１
Ｌは、上限を設定してもよい。航者の場合、地磁気方位
センサか車体着磁によりあらぬ方位を出力する状態てあ
っても方位変化はないことを検出できる点を活用出来る
。

また、角速度検出手段が車輪速センサを用いるものの場
合、速度検出手段は移動体の速度を車輪速センサデータ
に基ついて算出するものであってもよい。

また、学習係数ηは、例えば、装置のコールドスタート
時には比較的に大きな値、例えば１とし、その後は比較
的に小さく、例えば０．１にするなどのように可変定数
としてもよい。この場合、起動の直後などにおいて急速
に角速度補正係数を集束させることができる効果かある
。また、速度の関数としてもよい。この場合、高速度の
移動では道路状況の変化の速さに学習速度を追従させる
ことができるメリットなどかある。

〔発明の効果〕

以上のように、この発明によれば、移動体航法装置にお
ける角速度補正手段を、速度Ｖ、それまでの角速度補正
係数に基づいて補正された角速度δ、および絶対方位Ω
または上記移動体方位！の、所定距離以上の移動に伴う
変化角から、補正された角速度による方位変化の距離当
りの方位変化誤差Ｇを推定・演算し、この誤差Ｇに応じ
て所定の割合η（０くηく１）たけ、以後の角速度補正
係数を補正するようにしたので、角速度補正係数は入力
データの傾向・性質に順次適応し、しかも外乱（誤差）
の悪影響が後の角速度データへはとんと反映されること
なく補正され、その結果、格段に正確な方位例いては大
幅に正確な位置を得られることかコンピュータシミュレ
ーションおよび試作機を搭載した移動体の走行実験によ
り確認することができた。

【図面の簡単な説明】

第１図はこの発明の一実施例による移動体航法装置を示
すブロック図、第２図は移動距離に伴って累積誤差が増
大するような車輪速センサの性質を絶対方位センサのそ
れと比較して示す特性図、及び第３図はこの発明の他の
実施例による移動体航法装置を示すブロック図である。 図において、（１）は角速度補正手段、（２）は角速度
検出手段、（３）は絶対方位検出手段、（４）は速度検
出手段、（５）は方位演算手段、（６）は位置演算手段
である。（１１）は角速度補正演算部、（１２）は相対
方位変化演算部、（１３）は移動距離演算部、（１４）
は誤差演算部、（１５）は補正係数演算部である。 なお、図中、同一符号は同一、又は相当部分を示す。

Claims (1)

【特許請求の範囲】
1. 　移動体の絶対方位Ωを検出する絶対方位検出手段、上
   記移動体の角速度δを検出する角速度検出手段、上記移
   動体の速度Ｖを検出する速度検出手段、これらの検出デ
   ータおよび所定の角速度補正係数に基づいて上記角速度
   δを補正する角速度補正手段、補正された角速度δ、上
   記速度Ｖ、および上記絶対方位Ωに基づいて移動体方位
   Ψを演算する方位演算手段、並びに上記移動体方位Ψお
   よび上記速度Ｖに基づいて上記移動体の位置を演算・表
   示する位置演算・表示手段を備えた移動体航法装置にお
   いて、上記角速度補正手段は、上記速度Ｖ、それまでの
   角速度補正係数に基づいて補正された角速度δ、および
   上記絶対方位Ωまたは上記移動体方位Ψの、所定距離以
   上の移動に伴う変化角から、補正された角速度による方
   位変化の距離当りの方位変化誤差Ｇを推定・演算し、こ
   の誤差Ｇに応じて所定の割合η（０＜η＜１）だけ、以
   後の角速度補正係数を補正することを特徴とする移動体
   航法装置。