JPH09243383A - 移動体位置速度算出装置 - Google Patents

Abstract

(57)【要約】 【課題】 慣性航法計算で移動体(車、人)に関する位
置、速度を計算する移動体位置速度算出装置において、
慣性航法計算の発散を防止して、精度のよい位置、速度
を計算する。 【解決手段】 慣性航法の積分計算から求められる速度
とは別個に、進行方向速度検出手段５で、移動体が回転
運動するときに発生する遠心力方向の加速度とその回転
角速度から進行方向の速度を算出し、慣性航法速度補正
手段６は、その進行方向速度に基づいて補正度合いを決
定し、上記慣性航法計算で求めた速度の値を補正する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、例えば車や人間の
ように地上を移動する移動体の絶対位置、絶対速度を動
的に算出する移動体位置速度算出装置に関するもので、
カーナビゲーションシステムや人間位置検出装置(マン
ロケータ)や自動運転装置等に広く利用され得るもので
ある。

【０００２】

【従来の技術】従来の慣性航法手段を利用した移動体位
置速度算出装置としては、例えば、(文献１)日本航空宇
宙学会誌 第41巻 第468号(1993年１月)「航空宇宙技
術研究所におけるＧＰＳ航法飛行実験について(村田、
新宮、小野、張替)」に公開されている。

【０００３】以下、図４を参照しながら、従来の移動体
位置速度算出装置の一例について説明する。図４におい
て、１は慣性航法センサ、２は慣性航法手段、３は絶対
位置及び絶対速度検出手段、４は誤差推定手段である。

【０００４】次に、上記構成の移動体位置速度算出装置
について、その動作を説明する。まず、移動体に設置し
た慣性航法センサ１から移動体の移動に関するセンサデ
ータを得る。例えば、加速度センサから移動体の３軸方
向の加速度データを、ジャイロから３軸回りの角速度デ
ータをそれぞれ検出する。

【０００５】次に、上記センサデータを、後述する誤差
推定手段４の出力であるセンサ誤差補正値で補正した
後、慣性航法手段２で積分計算を中心とした慣性航法計
算を行うことにより、移動体の姿勢(ピッチング、ヨー
イング、ローリング)、絶対位置、絶対速度を計算す
る。慣性航法計算は基本的には初期値からの相対速度・
位置計算であるが、後述する誤差推定手段４の出力であ
る絶対位置、絶対速度への補正値で補正することによっ
て、絶対位置、絶対速度を出力することができる。

【０００６】例えば、慣性航法手段２としては、移動体
に慣性航法センサ１が固定されている場合には、移動体
の姿勢、速度、位置を全て計算によって求めるストラッ
プダウン方式を用いるのがよいが、ストラップダウン方
式については、航空機分野で既に商品化、実用化されて
いるアルゴリズムを用いればよく、ここでは詳説しな
い。

【０００７】さて、絶対位置及び絶対速度は、絶対位置
及び絶対速度検出手段３により検出する。例えば、絶対
位置及び絶対速度検出手段３としてはＧＰＳが用いら
れ、地球固定座標系に対する絶対位置と絶対速度(進行
方位も含めたベクトル値)が得られる。ただし、絶対位
置及び絶対速度検出手段３は、電波が遮断されれば全く
検出できなくなるし、遮断がなくともサブ秒か秒単位の
離散的な機会にしか結果が得られないのが一般的である
ので、常時、知りたいときに位置、速度のデータが得ら
れるとは限らないという欠点はある。

【０００８】誤差推定手段４は、絶対位置及び絶対速度
検出手段３で得られた絶対位置、絶対速度を観測値とし
て、慣性航法手段２の入力である慣性航法センサデータ
の誤差と慣性航法手段２の出力である姿勢、速度、位置
の誤差を補正する。

【０００９】具体的には慣性航法センサ誤差と慣性航法
手段２の出力誤差との間のダイナミクス(依存関係を表
した微分方程式)を定義し、観測量である絶対位置及び
絶対速度検出手段３で得られる位置・速度と慣性航法手
段２の出力である位置、速度との誤差情報から、上記ダ
イナミクスから推測する誤差の影響を最小とするよう
に、カルマンフィルタにより慣性航法センサの誤差、出
力である姿勢、速度、位置誤差を推定する。またこの誤
差推定処理の詳細は、例えば前記(文献１)にも出ている
ので、ここではこれ以上触れない。

【００１０】以上の構成により、ＧＰＳのような絶対位
置及び絶対速度検出手段で得られた観測値を使って補正
しながら、加速度センサやジャイロセンサのような慣性
航法センサ値を使って慣性航法計算を行い、ほぼ連続的
に姿勢、速度、位置を出力できる。また、仮にＧＰＳが
長時間受信できなくなっても、その直前までの補正デー
タを利用することで慣性航法を続行して、姿勢、速度、
位置を出力し続けることができる。

【００１１】

【発明が解決しようとする課題】しかしながら、上記の
ような構成の移動体位置速度算出装置では、慣性航法で
得られる速度に関して、直接補正できる手段がＧＰＳし
かないため、ＧＰＳが受信できなくなると、加速度セン
サやジャイロセンサの誤差を積分した速度に誤差が重畳
し、求めた移動体の速度値が発散してしまう危険性があ
った。

【００１２】本発明は、このような従来の問題点を解決
するものであり、慣性航法計算の発散を防止して、精度
のよい位置、速度を計算する移動体位置速度算出装置を
提供することを目的とする。

【００１３】

【課題を解決するための手段】上記目的を達成するため
に、本発明の移動体位置速度算出装置は、移動体が回転
運動するときに発生する遠心力方向加速度と、その回転
角速度から進行方向の速度を算出し、その進行方向の速
度により上記慣性航法計算で求めた速度の値を補正する
ように構成したものである。

【００１４】 従って、本発明は、１個以上の加速度
センサ、ジャイロセンサ等の慣性航法センサの出力を積
分して慣性航法計算により位置、速度を算出する移動体
位置速度算出装置であって、移動体が回転運動するとき
に発生する遠心力方向加速度とその回転角速度から進行
方向の速度を算出する進行方向速度検出手段と、その進
行方向速度より、上記慣性航法計算で求めた速度の値を
補正する慣性航法速度補正手段とを設けたものである。
これにより、慣性航法計算の速度誤差発散を防ぐことが
でき、ひいては、正確な移動体の速度、位置を求めるこ
とができる。

【００１５】 上記において、慣性航法速度補正手
段は、回転運動時に発生する回転角速度の絶対値の大小
により、進行方向速度検出手段で求めた速度による慣性
航法計算で求めた速度への補正の度合い(ゲイン)を変え
ることを特徴とするものであり、回転角速度の絶対値が
大きいときには求まる進行方向速度のＳ／Ｎ、精度が向
上することを利用して、慣性航法計算で求めた速度への
補正の度合い(ゲイン)を大きくし、逆に回転角速度の絶
対値が小さいときには補正の度合いを小さくすることに
より、精度のよい速度補正が実現できる。

【００１６】 上記において、慣性航法速度補正手
段は、回転運動するときに発生する遠心力方向加速度と
回転角速度との間の相互相関関数を一定時間の間計算す
る相互相関値計算手段を有し、上記相互相関値計算手段
で計算された相互相関値が大きいときには、進行方向速
度検出手段で求めた速度による慣性航法計算で求めた速
度への補正の度合い(ゲイン)を増大させ、逆に相互相関
値が小さいときには補正の度合いを減少させるものであ
り、雑音の中から遠心力成分だけを抽出して進行方向の
速度を求めることができるため、精度のよい速度補正が
実現できる。

【００１７】

【発明の実施の形態】以下、本発明の実施の形態につい
て詳細に説明する。 (実施の形態１)図１は、本発明の実施の形態１における
移動体位置速度算出装置の概略構成を示したもので、１
は慣性航法センサ、２は慣性航法手段、３は絶対位置及
び絶対速度検出手段、４は誤差推定手段、５は進行方向
速度補正手段、６は慣性航法速度補正手段である。

【００１８】次に、上記構成の移動体位置速度算出装置
について、その動作を説明する。まず、移動体に設置し
た慣性航法センサ１から移動体の移動に関するセンサデ
ータを得る。例えば、加速度センサから移動体の３軸方
向の加速度データを、ジャイロから３軸回りの角速度デ
ータをそれぞれ検出する。

【００１９】次いで、上記センサデータを、誤差推定手
段４の出力であるセンサ誤差補正値で補正した後、慣性
航法手段２で積分計算を中心とした慣性航法計算を行う
ことにより、移動体の姿勢(ピッチング、ヨーイング、
ローリング)、絶対位置、絶対速度を計算する。慣性航
法計算は基本的には初期値からの相対速度・位置計算で
あるが、誤差推定手段４の出力である絶対位置、絶対速
度への補正値で補正することによって、絶対位置、絶対
速度を出力することができる。

【００２０】例えば、慣性航法手段２としては、移動体
に慣性航法センサ１が固定されている場合には、移動体
の姿勢、速度、位置を全て計算によって求めるストラッ
プダウン方式を用いるのがよいが、ストラップダウン方
式については、航空機分野で既に商品化、実用化されて
いるアルゴリズムを用いればよく、ここでは詳説しな
い。

【００２１】さて、絶対位置及び絶対速度は、絶対位置
及び絶対速度検出手段３により検出する。例えば、絶対
位置及び絶対速度検出手段３としてはＧＰＳが用いら
れ、地球固定座標系に対する絶対位置と絶対速度(進行
方位も含めたベクトル値)が得られる。ただし、絶対位
置及び絶対速度検出手段３は、電波が遮断されれば全く
検出できなくなるし、遮断がなくともサブ秒か秒単位の
離散的な機会にしか結果が得られないのが一般的である
ので、慣性航法の補完的な役割として使われる。

【００２２】誤差推定手段４は、絶対位置及び絶対速度
検出手段３で得られた絶対位置、絶対速度を観測値とし
て、慣性航法手段２の入力である慣性航法センサデータ
の誤差と慣性航法手段２の出力である姿勢、速度、位置
の誤差を補正する。

【００２３】具体的には慣性航法センサ誤差と慣性航法
手段２の出力誤差との間のダイナミクス(依存関係を表
した微分方程式)を定義し、観測量である絶対位置及び
絶対速度検出手段３で得られる位置・速度と慣性航法手
段２の出力である位置、速度との誤差情報から、上記ダ
イナミクスから推測する誤差の影響を最小とするよう
に、カルマンフィルタにより慣性航法センサの誤差、出
力である姿勢、速度、位置誤差を推定する。

【００２４】以上は、図４で示した従来例の場合と同様
である。以下、本発明の特徴部について説明する。

【００２５】進行方向速度検出手段５により、慣性航法
手段２とは別個に進行方向速度を算出する。例えば、乗
用車がカーブを曲がった時を想定すると、カーブを曲が
ることにより、ヨーイング方向の遠心力が働く。そこ
で、その遠心力方向加速度をａｙ、ヨーイング方向の角
速度をｗｙとすると、進行方向速度ｖｘは、

【００２６】

【数１】ｖｘ＝ａｙ／ｗｙ で求めることができる。よって、ヨーイング方向のジャ
イロと遠心力方向(即ち、進行方向とは直交する方向)の
加速度センサの値を用いて、進行方向の速度を求めるこ
とができる。

【００２７】なお、遠心力は、例えば、坂を上る瞬間の
ようにピッチング方向にも生じることがあり、そのとき
も同様に進行方向の速度を求めることができるので、上
記進行方向速度検出手段５はヨーイング方向の回転だけ
に限定したものではない。

【００２８】次に、慣性航法速度補正手段６は、進行方
向速度検出手段５で得られた進行方向速度により、慣性
航法で計算された速度を補正する。その補正方法として
は、例えば、確実にカーブを曲がったとき、あるいは、
坂を上り始めたことを検出して、そのときに進行方向速
度検出手段５で計算された進行速度を、慣性航法の速度
計算の初期値にしたり、あるいは、実施の形態２，３で
説明するように、曲がった角度、雑音のレベルより、補
正の度合いを変え、徐々に、しかし、ほぼ常時にわたっ
て補正する方法も考えられる。

【００２９】以上の構成により、仮にＧＰＳのような絶
対位置及び絶対速度検出手段３からの情報が長時間途切
れたとしても、慣性航法の速度誤差が発散して暴走する
のを防ぎ、また、精度のよい移動体進行速度を得ること
ができる。

【００３０】(実施の形態２）図２は、実施の形態２に
おける移動体位置速度算出装置の一構成要素である慣性
航法速度補正手段の説明のための図である。装置の構成
及び、慣性航法速度補正手段以外の動作は実施の形態１
の場合と同一であるから、慣性航法速度補正手段につい
てのみ説明する。

【００３１】進行方向速度検出手段５で求められた進行
方向速度は、遠心力方向加速度を回転角速度で除するこ
とで得られるので、一般に角速度の絶対値が小さいほど
精度は低い。逆に、角速度の絶対値が大きいときは、推
定される速度の精度も高いと考えてよい。よって、遠心
力方向加速度から得られる速度で、慣性航法速度を補正
する際には、角速度の絶対値に注目し、その値によって
補正の度合いを変えればよい。

【００３２】また、逆に、遠心力が発生するのは大きく
曲がったときだけでなく、緩いカーブや、極端な場合、
道路が直線であっても、車の微小なうねり走行によって
遠心力は発生しているはずである。むろん、精度やＳ／
Ｎの点で信頼度は落ちるが、その分だけ補正の度合いを
落すことによって、僅かずつであっても慣性航法速度を
補正することができる。

【００３３】図２（ａ）は、回転角速度の時間的な変動
を示したものであり、図２(b)は、慣性航法速度補正ゲ
インの時間的な変動を示したものである。回転角速度の
大小によって補正ゲインを増大させたり、減少させたり
していることがわかる。

【００３４】補正は、例えば、進行方向速度をｖｘ、慣
性航法速度をｖｉ、慣性航法速度補正ゲインをｇとする
と、

【００３５】

【数２】ｖｉ＝ｖｉ＋ｇ(ｖｘ−ｖｉ) で実現することができる。

【００３６】以上の構成により、角速度の絶対値から速
度補正のゲインを適応的に変えることによって、慣性航
法の速度誤差が発散して暴走するのを防ぎ、また、精度
のよい移動体進行速度を得ることができる。

【００３７】(実施の形態３)図３は、実施の形態３にお
ける移動体位置速度算出装置の一構成要素である慣性航
法速度補正手段の説明のための図である。装置の構成及
び、慣性航法速度補正手段以外の動作は実施の形態１の
場合と同一であるから、慣性航法速度補正手段について
のみ説明する。

【００３８】進行方向速度検出手段５で求められた進行
方向速度は、遠心力方向加速度を回転角速度で除するこ
とで得られる。逆にもし進行方向速度が精度よく得られ
るならば、そのときの遠心力方向加速度と回転角速度の
データには強い相関があるはずである。そこで、ある一
定時間(乗用車に適用する場合、0.1秒から0.5秒程度が
適当)の遠心力方向加速度と回転角速度との間の相互相
関関数を計算する。相互相関関数Ｒawは、

【００３９】

【数３】

【００４０】で求めることができる。なお、遠心力方向
加速度を検出するセンサデータと回転角速度を検出する
センサデータとが同期がとれているならば、(数３)はｔ
は０でよい。

【００４１】この相関関数値が大きいときは両者に相関
がある。即ち、遠心力方向加速度、回転角速度から進行
方向速度が精度よく求められると考えてよいので、補正
の度合いを大きくすればよい。

【００４２】図３(a)は、相関関数値の時間的な変動を
示し、図３(b)は、慣性航法速度補正ゲインの時間的な
変動を示したものである。相関関数値の大小によって補
正ゲインを増大させたり、減少させたりしていることが
わかる。

【００４３】具体的な補正の式自体は、実施の形態２の
場合と同様で、例えば、進行方向速度をｖｘ、慣性航法
速度をｖｉ、慣性航法速度補正ゲインをｇとすると、前
記(数２)で実現することができる。

【００４４】以上の構成により、遠心力方向加速度と回
転角速度との相互相関値の大きさから速度補正のゲイン
を適応的に変えることによって、慣性航法の速度誤差が
発散して暴走するのを防ぎ、また、精度のよい移動体進
行速度を得ることができる。

【００４５】

【発明の効果】以上説明したように、上記の構成によ
れば、仮にＧＰＳのような絶対位置及び絶対速度検出手
段からの情報が長時間途切れたとしても、移動体に発生
する遠心力から絶対速度を算出し、その速度値により慣
性航法の速度を補正することにより、慣性航法の速度誤
差が発散して暴走するのを防ぐことができ、また、精度
のよい移動体進行速度を得ることができる。

【００４６】また、の構成によれば、角速度の絶対値
から速度補正のゲインを適応的に変え、速度補正の信頼
性を上げることによって、慣性航法の速度誤差が発散し
て暴走するのを防ぎ、また、精度のよい移動体進行速度
を得ることができる。

【００４７】さらに、の構成によれば、遠心力方向加
速度と回転角速度との相互相関値の大きさから速度補正
のゲインを適応的に変え、雑音の影響を極力抑えて速度
補正の信頼性を上げることによって、慣性航法の速度誤
差が発散して暴走するのを防ぎ、また、精度のよい移動
体進行速度を得ることができる。

【図面の簡単な説明】

【図１】本発明の実施の形態１における移動体位置速度
算出装置の概略構成図である。

【図２】本発明の実施の形態２における移動体位置速度
算出装置を構成する慣性航法速度補正手段の動作説明図
である。

【図３】本発明の実施の形態３における移動体位置速度
算出装置を構成する慣性航法速度補正手段の動作説明図
である。

【図４】従来例の移動体位置速度算出装置の概略構成図
である。

【符号の説明】

１…慣性航法センサ、 ２…慣性航法手段、 ３…絶対
位置及び絶対速度検出手段、 ４…誤差推定手段、 ５
…進行方向速度検出手段、 ６…慣性航法速度補正手
段。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 辻 弘彰 神奈川県横浜市港北区綱島東四丁目３番１ 号 松下通信工業株式会社内 (72)発明者 前田 裕幸 神奈川県横浜市港北区綱島東四丁目３番１ 号 松下通信工業株式会社内

Claims (3)

【特許請求の範囲】
1. 【請求項１】 １個以上の加速度センサ、ジャイロセン
   サ等の慣性航法センサの出力を積分して慣性航法計算に
   より位置、速度を算出する移動体位置速度算出装置であ
   って、 移動体が回転運動するときに発生する遠心力方向加速度
   とその回転角速度から進行方向の速度を算出する進行方
   向速度検出手段と、 その進行方向速度より、上記慣性航法計算で求めた速度
   の値を補正する慣性航法速度補正手段とを設けたことを
   特徴とする移動体位置速度算出装置。
2. 【請求項２】 慣性航法速度補正手段は、回転運動時に
   発生する回転角速度の絶対値の大小により、進行方向速
   度検出手段で求めた速度による慣性航法計算で求めた速
   度への補正の度合い(ゲイン)を変えることを特徴とする
   請求項１記載の移動体位置速度算出装置。
3. 【請求項３】 慣性航法速度補正手段は、回転運動する
   ときに発生する遠心力方向加速度と回転角速度との間の
   相互相関関数を一定時間の間計算する相互相関値計算手
   段を有し、上記相互相関値計算手段で計算された相互相
   関値が大きいときには、進行方向速度検出手段で求めた
   速度による慣性航法計算で求めた速度への補正の度合い
   (ゲイン)を増大させ、逆に相互相関値が小さいときには
   補正の度合いを減少させることを特徴とする請求項１記
   載の移動体位置速度算出装置。