(57)【特許請求の範囲】
【請求項１】
陸ベース車両の作動点を正確に求めるためのコンピュータベースのシステム（100）であって、
衛星ベース位置システム（218）から航法信号を受信し、該航法信号を用いて車両の受信機判定位置を求めるための、前記車両に取付けられた受信機手段（216）と、
前記車両の少なくとも一つの動的パラメータを計測するセンサー手段（230,232,234,236）と、
前記受信機判定位置と前記センサー手段（230,232,234,236）からの少なくとも一つの動的パラメータとを組み合わせて、推定される位置を含む前記作動点を求めるためのカルマンフィルタ手段（208）と、
が設けられており、
前記カルマンフィルタ手段（208）は、前記センサー手段（230,232,234,236）が、前記車両が静止状態であることを示すときに静止カルマンフィルタモデルに変わり、前記センサー手段（230,232,234,236）が前記車両が動いていることを示すときに移動カルマンフィルタモデルに変わる手段を有し、前記受信機判定位置の適時性に影響を及ぼすような、前記受信機手段に関連したタイムラグをモデル化する手段を含むものであり、
前記カルマンフィルタ手段（208）は、前記モデル化手段と前記受信機判定位置とを用いて改善された推定位置を形成するようになっていることを特徴とするコンピュータベースのシステム（100）。
【請求項２】
陸ベース車両の作動点を正確に求めるためのコンピュータベースのシステム（100）であって、
衛星ベース位置システム（218）から航法信号を受信し、該航法信号を用いて車両の受信機判定位置を求めるための、前記車両に取付けられた受信機手段（216）と、
前記車両の少なくとも一つの動的パラメータを計測するセンサー手段（230,232,234,236）と、
前記受信機判定位置と前記センサー手段（230,232,234,236）からの少なくとも一つの動的パラメータとを組み合わせて、推定される位置を含む前記作動点を求めるためのカルマンフィルタ手段（208）と、
が設けられており、
前記カルマンフィルタ手段（208）は、前記センサー手段（230,232,234,236）が前記車両が静止状態であることを示すときに前記作動点の良好な判定が得られるように、推定された受信機判定北の位置と、推定された受信機判定東の位置と、前記推定される北の位置および東の位置を含むカルマンフィルタ状態を含む静止カルマンフィルタモデルに変わる手段と、前記センサー手段（230,232,234,236）が前記車両が動いていることを示すときに、推定された受信機判定北の位置と、推定された受信機判定東の位置と、推定された受信機判定北の速度と、推定された受信機判定東の速度と、前記推定北の位置と、前記推定東の位置と、推定向き変更と、推定向き変更率及び推定速さを含むカルマンフィルタ状態を含む移動カルマンフィルタモデルに変わる手段とを含んでいることを特徴とするコンピュータベースのシステム（100）。
【請求項３】
陸ベース車両の作動点を正確に求めるためのコンピュータベースのシステム（100）であって、
衛星ベース位置システム（218）から航法信号を受信し、該航法信号を用いて車両の受信機判定位置を求めるための、前記車両に取付けられた受信機手段（216）と、
前記車両の少なくとも一つの動的パラメータを計測するセンサー手段（230,232,234,236）と、
前記受信機判定位置と前記センサー手段（230,232,234,236）からの少なくとも一つの動的パラメータとを組み合わせて、推定される位置を含む前記作動点を求めるためのカルマンフィルタ手段（208）と、
が設けられており、
前記カルマンフィルタ手段（208）は、前記センサー手段（230,232,234,236）が前記車両が静止状態であることを示すときに前記作動点の良好な判定が得られるように、推定された受信機判定北の位置と、推定された受信機判定東の位置と、前記推定される北の位置および東の位置を含むカルマンフィルタ状態を含む静止カルマンフィルタモデルに変わる手段と、前記センサー手段（230,232,234,236）が前記車両が動いていることを示すときに、推定された受信機判定北の位置と、推定された受信機判定東の位置と、推定された受信機判定北の速度と、推定された受信機判定東の速度と、前記推定北の位置と、前記推定東の位置と、推定向き変更と、推定向き変更率及び推定速さを含むカルマンフィルタ状態を含む移動カルマンフィルタモデルに変わる手段とを含んでおり、さらに前記カルマンフィルタ手段（208）は、前記受信機手段（216）に対応するタイムラグが前記受信機判定位置に及ぼす影響を判定し、モデル化する手段を含んでおり、
前記受信機手段（216）は、前記車両の受信機判定速度も求めるようになっており、
前記カルマンフィルタ手段（208）が、前記受信機判定位置と、前記センサー手段（230,232,234,236）及び前記受信機判定速度を組み合わせて前記作動点を求めるようになっているものであることを特徴とするコンピュータベースのシステム（100）。
【発明の詳細な説明】
技術分野
本発明は、一般的に自律式車両の制御に関する。より詳細には、本発明は、自律式車両の正確な作動点を求めるためのシステムと方法に関する。
背景技術
イリノイ州ペオリア在住のキャタピラー社は、オフロード炭鉱用車両を製造している。本件出願人によって１９９５年２月１４日に許可された米国特許第５、３９０、１２５号の“車両位置決定システム及び方法”では、炭鉱用車両に用いるための自律式車両システムを開示する。本発明は、この先行技術を引用し、この特許明細書の記述を本明細書の記述の一部とする。
自律性を達成するために、車両は、常にその状況に対する位置を求めることができなければならない。米国特許第５、３９０、１２５号に開示された車両システムは、車両位置システム（ＶＰＳ）を用いて車両の位置を求める。ＶＰＳは、第１位置推定値と第２位置推定値の重み付け平均を行なうことによって車両の位置の正確な推定値を求める。第１位置推定値は全地球的航法（ＧＰＳ）受信機から受け取られる。第２位置推定値は、慣性基準ユニットから受け取られる。第１及び第２位置推定値は予想される精密さの関数として重み付けされて、より正確な第３の位置推定値を作り出す。米国特許第５、３９０、１２５号に開示されるようなシステムは非常に高価である。例えば、慣性基準ユニットだけでも、約１０、０００ドルから１００、０００ドルかかる。
位置決めのために、ＧＰＳ受信機だけを用いることは可能である。しかしながら、高度な精密さが要求されるときには、集積型位置システムが好ましい。集積型位置システムは、いくつかの異なる種類のセンサーからの計測値を用いて、かなり正確な位置情報を得ることができる。集積型位置システムの多くの例が知られている。このような集積型システムは、ＧＰＳ航法信号とともに、慣性及び別の車両運動型センサーからの計測値を用いてより正確な位置推定値を作り出す。しかしながら、これらのシステムは、一般的にカスタム設計であるために、これを達成するには費用もかかり厄介である。
費用の点で有効な手段で正確な位置推定値を作り出せるシステムが必要とされる。このようなシステムは比較的費用が安く、商業的にも入手可能な部品を使用するのが好ましい。
発明の開示
本発明は、炭鉱用トラックのような自律式車両の作動点を正確に求めるためのシステムと方法である。トラック基準ユニット（ＴＲＵ）は、車両の作動点を求める。“作動点”とは、車両の動的な位置を表すパラメータの数のことをいう。例えば、これらの動的なパラメータは、位置、速度、加速度、速さ、向き変更、向き変更率、ピッチ、ピッチ率、ロール、及びロール率を含んでいればよいが、これらに限定されない。好ましい実施例において、作動点は、北の位置、東の位置、速さ、向き変更及び向き変更率を含む。
好ましい実施例において、本発明は自律式車両システムにおいて達成される。トラック基準ユニット（ＴＲＵ）は車両の作動点を求める。ナビゲータはＴＲＵから作動点を受け取り、作動点情報を用いて車両を誘導する。
ＴＲＵは、いくつかの商業的に入手可能なセンサーから受け取られた計測値を組み合わせて、車両の作動点を求める。好ましい実施例において、これらのセンサーは、衛星位置システム受信機、速さインジケータ、オドメータ、リゾルバ、及び角速度センサーを含む。これらの計測値は重み付けされ、組み合わされて作動点の正確な推定値を与える。
ＴＲＵは、推定値の質が低下するかもしれないが、どれか一つのセンサーがなくてもナビゲータに作動点の推定値を与えるＴＲＵの能力を失わないように設計されている。これによって、センサーがオンライン状態に戻る、即ちセンサーが作動するまで、ナビゲータは所望のルートに沿って自律式車両に命令し続けることができる。
好ましい実施例において、ＴＲＵは、全地球航法（ＧＰＳ）受信機から北の位置、北の速度、東の位置及び東の速度の測定値を、２方向性オドメータから移動した距離と方向を、実対地速度ドップラレーダから速度を、リゾルバからかじ取り角を、固体状態の角速度センサーから向き変更率を受信するコンピュータベースシステムである。ＴＲＵは、９つの状態か４つの状態に拡張したカルマンフィルタのいずれかを用いて計測値を組み合わせて、車両の作動点の推定値を計算する。９状態に拡張したカルマンフィルタは車両が移動するときに用いられる。４状態に拡張したカルマンフィルタは、車両が静止しているときに用いられる。
本発明の前述及び他の特徴及び利点は、添付した図面に示すように、次の本発明の好ましい実施例のより詳細な記載から明らかになるであろう。
【図面の簡単な説明】
図１は、本発明の自律式車両を表す高レベルのブロック線図である。
図２は、本発明のトラック基準ユニットの構造を表すブロック線図である。
図３は、本発明のトラック基準ユニットの作動を表すフローチャートである。
図４は、本発明のトラック基準ユニットによって実施されるフィルタリング作動を表すフローチャートである。
本発明を実施するのに最良の形態
本発明の好ましい実施例を以下に詳細に述べる。特定部分の符号と形状に関して述べているが、これは図示を目的にするだけのものである。本分野における当業者であれば、別の成分及び形状も本発明の精神と範囲から逸脱することなく用いてもよいことがわかるであろう。
本発明の好ましい実施例は、図面を参照して記載されており、同じ参照符号は同じ要素を表す。さらに、各参照番号の最も左側の数字は、この番号が最初に用いられた図を示している。
産業上の利用可能性
本発明は、自律式車両の作動点を正確に求めるためのシステム及び方法に関する。“作動点”とは、動的な車両位置のことをいう。車両の動的位置を表す動的パラメータは、例えば位置、速度、加速度、向き変更、ピッチ、ロール、速さ、向き変更率、ピッチ率、およびロール率を含む。好ましい実施例において、作動点は、北の位置、東の位置、速度、向き変更及び向き変更率を含む。図示のために、本発明を自律式車両システムの状況で記載する。これは図示の目的のためにのみなされる。本分野における当業者であれば、本発明を別の用途に用いてもよいことがわかるであろう。
図１は、自律式車両システム１００を表す高レベルのブロック線図である。自律式車両システム１００は、車両群管理機構１０２、車両制御システム１０４、及びテレオペレーションパネル１０６を含む。車両群管理機構１０２は、ダンプトラックのような一群の自律式炭鉱用車両を管理するように構成されている。車両群管理機構１０２は職長のように作用し、炭鉱用車両に作業を割当て、車両が作業を行なうときの進行状態を追跡する。車両群管理機構１０２は無線リンク１０８によって各車両と通信する。各車両は、オンボード車両制御システム１０４を含む。車両制御システム１０４は、車両群管理機構１０２の制御のもとで炭鉱用車両の自律操作を行なうことができる。車両制御システム１０４は、ナビゲータ１２０、トラック基準ユニット（ＴＲＵ）１２２、障害物検出機１２４、機械制御モジュール（ＭＣＭ）１２６、進歩したディーゼルエンジン管理機構（ＡＤＥＭ）１２８、電子プログラム可能トランスミッション機構（ＥＰＴＣ）１３０、および必須情報管理機構（ＶＩＭＳ）１３２を含む。
ナビゲータ１２０は、無線リンク１０８を介して車両群管理機構１０２から指令を受け取る。この指令は、例えば仕事の作業割当を含む。ナビゲータ１２０は次に進むべきルートを仕事から決定する。例えば、このルートは、オープンピット鉱山操作における掘削場所と破砕場所との間の運搬部分である。
ＴＲＵ１２２は、衛星ベースの位置システムとオンボードセンサーから求められた計測値を用いて車両の作動点を求める。車両の作動点と所望のルートに基づいて、ナビゲータ１２０は車両に対して所望のかじ取り角と所望の速度を作り出す。障害物検出機１２４は、車両前部の領域を障害物に対して走査するレーダユニットである。障害物検出機１２４が障害物を検出すると、ナビゲータ１２０に障害物が検出されたことを知らせたり、または障害物の位置を知らせる。ナビゲータ１２０は、車両を停止させたり、または障害物の回りを誘導する。
テレオペレーション１０６は、１１０で示されているような無線信号を介してかじ取り角、速度及び他の命令をナビゲータ１２０に直接通信するのに用いられて、車両の遠隔制御操作を行なうことができる。
車両群管理機構１０２、ナビゲータ１２０、及び障害物検出器１２４の作動については、上述で組み込んだ米国特許第５、３９０、１２５号に詳細に記載されている。
ナビゲータ１２０、ＴＲＵ１２２及び障害物検出器１２４は、車両オンボード情報を表し、自律制御コマンドを速度及びかじ取り角コマンドの形態で作り出せる。ナビゲータ１２０は、所望の移動ルートと車両の作動点に基づいて速度及びかじ取り角コマンドを作り出す。ナビゲータ１２０は、現在の作動点に基づいて現在のかじ取り角と速度の調整を計算し、車両を所望のルートに沿って動かす。車両群管理機１０２は、ナビゲータ１２０に所望のルートを与える。ＴＲＵ１２２はナビゲータ１２０に自律式車両の作動点を与える。
ＴＲＵ１２２の構造を表すブロック線図を図２に示す。好ましい実施例において、ＴＲＵ、ＴＲＵセンサインターフェイス２０２、ＴＲＵディジタル入力カード２０４、ＴＲＵアナログ入力カード２０６、ＴＲＵプロセッサ２０８、ＴＲＵ直列インターフェイス２１０及びＴＲＵデータ母線２１２を含む。センサーインターフェイス２０２は、２方向オドメータ２３０、ドプラ対地速度インジケータ２３２、固形状態の向き変更率センサ２３４、及びかじ取りリゾルバ２３６から信号を受信する。２方向性オドメータ２３０は、車両が移動する距離と方向、即ち前進または後退かの方向を計測する。ドプラ対地速度インジケータ２３２は、車両が進行する速度を計測する。固体状態の向き変更率センサ２３４は、車両が向き変更、即ち向きを変えるときの割合を計測する。かじ取りリゾルバ２３６は車両のかじ取り角を計測する。本分野の当業者であれば、様々な別のセンサーを同じパラメータを計測するのに用いてもよいことがわかる。例えば、タコメータは速度を計測するのに用いることができたり、またはジャイロスコープを向き変更率を計測するのに用いることができる。
さらに、別の精密さを得ようとする際に、付加的な又は異なるセンサーから計測が行なわれて本発明に組み込まれる。例えば、上昇角を計測するのに傾斜計を用いて、ピッチ率センサーを上昇の変更率を計測するのに用いたり、またはコンパスは向き変更を計測するのに用いることができる。一方、精密さをいくらか失うという犠牲を払って、費用を節約するためにいくつかのセンサーを取り除くことができる。
センサー計測値は、センサーインターフェイス２０２によって収集される。センサーインターフェイス２０２は、例えば、タインミングストロブ、パルス幅変調信号等のようなディジタル信号をディジタル入力カード２０４に通過させ、電圧、電流等のアナログ信号をアナログ入力カード２０６に通過させる。ディジタル入力カード２０４は、ディジタルタイミングとパルス信号をディジタル値に変換してディジタルレジスタ２０５内に記録する。アナログ入力カード２０６は、アナログ信号を基準化してディジタル値に変換し、アナログレジスタ２０７に記憶する。ＴＲＵプロセッサ２０８、即ち一般的な目的のためのマイクロプロセッサは、ディジタルレジスター２０５、またはアナログレジスタ２０７にデータ母線２１２を介してアクセスしており、センサー計測値を得る。
ＴＲＵ１２２は衛星ベースの位置システム２１８から位置データを受け取る。好ましい実施例において、衛星ベースの位置システム２１８は、全地球航法（ＧＰＳ）である。ＧＰＳ受信機２１６は、衛星ベース位置システム２１８から衛星信号を受信しこれらの信号に基づいて位置を求める。ＧＰＳ受信機２１６は、北の位置、東の位置、北の速度および東の速度とともにタイミング情報を含む受信機が判定した計測値をＴＲＵ１２２に与える。ＴＲＵ１２２は、直列インターフェイス２１０を介してＧＰＳ受信機２１６から計測値を受信する。ＴＲＵプロセッサ２０８は、直列インタフェイス２１０からＧＰＳ受信機計測値を受け取る。ＧＰＳ受信機で得られた計測値と上述で得られたセンサー計測値で、ＴＲＵプロセッサ２０８は自律式車両の作動点の推定値を計算する。ＴＲＵプロセッサ２０８は、作動点を直列インターフェイスを通過させ、作動点を直列データライン２１４でナビゲータ１２０に送る。
残りの説明ではカルマンフィルタの簡単な説明を必要とする。一般的にカルマンフィルタは、最適な最小二乗平均の推定値である。好ましい実施例において、伸長したカルマンフィルタが用いられる。拡張したカルマンフィルタは、現在の状態の非線形システムに基づいて線形化したモデルを用いて最小平均二乗推定値を計算する。本明細書では“カルマンフィルタ”と“カルマンフィルタリング”を用いており、これは同様に“拡張したカルマンフィルタ”と“拡張したカルマンフィルタリング”にも適用できる。
カルマンフィルタリングは本分野においてよく知られている。これはＧＰＳ位置システムに該当する。実際、多くの引例では、カスタムの用途としてカルマンフィルタ、慣性航法ユニット、及びＧＰＳ位置システムを一体化して精密度の高い車両の車両位置と挙動を作り出すことを開示する。これらのシステムにおいて、中央プロセッサは、ＧＰＳ位置システムから動的位置を求め、慣性航法ユニット（ＩＮＵ）からの信号に基づいて動的位置を求め、ＧＰＳおよびＩＮＵサブシステムと対応した複雑化した命令と制御を履行することを含む作業を実施する。中央プロセッサは、カルマンフィルタリングを行なって、車両の作動点の推定値を得なければならない。これらのサブシステムは、広範囲の展開ととりまとめ時間を必要とし、多くの場合高価なハードウェアを必要とする。
しかしながら、本発明は、商業的に入手可能なＧＰＳ受信機と、車両の作動点を推定するための既製のセンサーを用いる。中央プロセッサ（即ちＴＲＵプロセッサ２０８）の役割は、ＧＰＳ受信機のようなセンサーからの計測値を収集して、カルマンフィルタリングを実施し、作動点を推定することである。部品の費用がより安いことに加えて、このシステムは、展開ととりまとめに必要とする時間は少ない。このように、本発明は、効率的にかつ費用も有効な手段で正確な作動点を作り出すように機能できる。
しかしながら、ＧＰＳ受信機があるフィルタリングを内的に実行するという事実のために、カルマンフィルタによって得られる推定値において精密さが幾分か損なわれる。これによって２つの問題が発生する。第１に、ＧＰＳ受信機におけるフィルタリングは、ＧＰＳ受信機が出力するデータにおいてタイムラグを導くことになる。このため、車両が静止しはじめるまで、ＧＰＳ受信機によって計算された位置と挙動が、一貫して実際の位置と動きに遅れることになる。車両が数秒の間停止したままであったとき、ＧＰＳ受信機によって計算される位置は車両の実際の位置に設定される。タイムラグは、カルマンフィルタにおいてモデル化されてこれを説明する。
好ましい実施例において、タイムラグは、カルマンフィルタの入力として用いられた４個のＧＰＳ受信機で決定された計測値のそれぞれごとに第１番目のラグとしてモデル化される。これによって、カルマンフィルタはどの推定値がタイムラグなしであるべきかを推定する。タイムラグをモデル化するのにカルマンフィルタにおいて４つの状態が必要である。４つの状態は、推定された受信機判定北位置、推定された受信機判定東位置、推定された受信機判定北速度、推定された受信機判定東速度である。この方法でＧＰＳ受信機をモデル化することによってカルマンフィルタを介してラグの影響を小さくして、車両の作動点の推定値がよりよくなる。
ＧＰＳ受信機によって実施されたフィルタリングの第２の影響は、ＧＰＳ受信機に入るホワイトノイズがカラー化したノイズとして受信機に残ったままとなることである。即ち、このノイズは相関関係が高い。カルマンフィルタに入るノイズがホワイトであるか非相関関係であるとして考えられるのでカルマンフィルタの性能をいくらか低下させる。広範囲の処理を行なうことなく、この問題を殆ど解決することはできない。この結果として、車両の作動点の正確な推定値がわずかに悪くなる。
カルマンフィルタを含むＴＲＵプロセッサ２０８によって実施された処理を表すフローチャートを図３に表す。処理は、各センサーによって与えられた計測値を読み取ることによって段階３０２において開始する。これを達成するためにＴＲＵプロセッサ２０８はディジタル入力カード２０４上でディジタルレジスタ２０５を読み取り、オドメータ２３０、速さインジケータ２３２、またはかじ取りリゾルバ２３６から計測値を受取り、アナログ入力カード２０６上でアナログレジスタ２０７を読み取り、向き変更率センサ２３４から測定値を受け取るようになっている。段階３０４において、ＴＲＵプロセッサ２０８は、段階３０２において得られた各測定値を較べ、この測定値が各センサの有効範囲にあることをチェックする。
判定段階３０６において、ＴＲＵプロセッサ２０８は直列インターフェイス２１０をチェックし、新しいＧＰＳ受信機測定値が存在するかどうかをチェックする。新しいＧＰＳ受信機測定値が存在する場合には、ＴＲＵプロセッサ２０８はＧＰＳ受信機測定値が段階３０８内において有効範囲内にあることをチェックする。有効範囲内にない測定値は無視する。新しいＧＰＳ受信機測定値が存在しない場合には、処理が段階３１０に続く。
判定段階３１２において、ＴＲＵプロセッサ２０８が直列インターフェイス２１０をチェックし、ナビゲータ１２０から新しい情報が存在するかを判定する。ナビゲータ１２０は、かじ取り角と速さコマンドに関するＴＲＵ１２２情報を送る。ナビゲータ１２０からのこの情報が存在する場合には、ＴＲＵプロセッサ２０８は段階３１２に進む。ナビゲータ１２０からの新しい情報がない場合には、処理が段階３１３に続く。
段階３１２において、ＴＲＵプロセッサ２０８はナビゲータ１２０から受け取った情報をチェックし、この情報が有効範囲内にあることを確認する。有効範囲内にない情報は無視されて、次の処理には使用されない。ナビゲータ１２０から受け取ったいかなる情報も確認したあと、ＴＲＵプロセッサ２０８は段階３１３にすすむ。
段階３１３において、ＴＲＵプロセッサ２０８は段階３０２において受け取ったセンサ測定値、または段階３０８において受け取られた受信機測定値と、現在の作動点の一致性をチェックする。これらの測定値のいずれもが現在の作動点と一致しない場合には、これらの不一致性の測定値は、段階３１４においてカルマンフィルタを更新するために無視される。例えば、速さインジケータ２３２が１００マイル／時間（ｍｐｈ）の速度測定値を示し、現在の作動点が１０ｍｐｈの速度を示す場合には、速さインジケータ２３２からの速さ測定値は、一回のフィルタ更新の間、速度がこの量によって変更されたとは考えられないので無視される。あり得る結果としては、測定値が信頼できないものであったり、またはセンサーが停止していることがある。信頼ある情報をカルマンフィルタにできるかぎり与えるために、不一致な測定値が無視される。ＴＲＵプロセッサ２０８が一致性の測定値をチェックし、不一致な計測値を無視した後、処理が段階３１４に続く。
段階３１４において、ＴＲＵプロセッサ２０８は、段階３０２において受け取ったセンサー測定値、または段階３０６において受け取られたＧＰＳ受信機測定値でカルマンフィルタを更新する。好ましい実施例において、センサー測定値は１０ヘルツ（Ｈｚ）で受け取られ、ＧＰＳ受信機測定値は１ヘルツで受け取られる。このため、カルマンフィルタは、ＧＰＳ受信機測定値で更新される場合と同じようにセンサ測定値で１０回更新される。カルマンフィルタが様々な源から、かつ異なる割合で測定値を処理するのに適することが本分野においてよく知られている。段階３１６において、ＴＲＵプロッセッサ２０８は、ナビゲータ１２０に自律式車両の作動点の新しい推定値を直列インターフェイス２１０を介して与える。
本発明は、車両が動いているかどうかによって異なるカルマンフィルタを用いる。車両が動いている場合には、本発明は９状態のカルマンフィルタを用いて、車両の作動点を推定する。９状態のカルマンフィルタは、動く車両のよりすぐれたモデルを作る。この明細書のために、９状態のカルマンフィルタモデルは移動用カルマンフィルタモデルといわれる。好ましい実施例において、移動用カルマンフィルタモデルを用いて推定される状態は、推定される北の位置、推定される東の位置、推定される向き変更、推定される向き変更率、推定される速さ及び４ＧＰＳ受信機測定値；推定される受信機判定北の位置、推定される受信機が判定する東の位置、推定される受信機が判定する北の速度、推定される受信機が判定する東の速度である。
車両が静止している場合には、本発明は、４状態カルマンフィルタを用いる。４状態カルマンフィルタは静止車両のよりすぐれたモデルを形成する。４状態カルマンフィルタモデルは、静止カルマンフィルタモデルといわれる。静止カルマンフィルタモデルを用いて推定される状態は、推定北の位置、推定東の位置、推定される受信機判定北の位置、および推定される受信機判定東の位置である。
車両が動かない（即ち静止状態）とき、フィルタモデルを変更することによって、カルマンフィルタは、センサーからのノイズの影響が不必要な測定値を与えることなく、より早く収束できる。例えば、速度の積分として位置がカルマンフィルタ内でモデル化される。積分の影響のために、速さ測定値の小さな誤差（例えばオフセット、移動等のために）のために位置の推定値の誤差が大きくなる。同様に、向き変更率計測値における小さな誤差のために向き変更推定値の誤差が大きくなる。カルマンフィルタは最終的に実際の作動点に収束されるときでさえ全体のシステムの応答がカルマンフィルタモデルを変更することによって高められる。
この場合、移動及び静止カルマンフィルタモデルは、静止モデルが移動モデルから状態の数を取り除くという点において異なる。好ましい実施例において、取り除かれる状態は、推定される受信機判定北の速度、推定される受信機判定東の速度、推定される向き変更、推定される向き変更率及び推定される速さである。最終的に、静止カルマンフィルタモデルは、静止車両をより正確にモデル化し、車両の作動点をより良好に推定することになる（静止の場合においては、結局車両の位置に等しくなる）。
車両が静止し始めると、本発明は、カルマンフィルタを移動カルマンフィルタモデルから静止カルマンフィルタモデルに切り変える。好ましい実施例においてオドメータは、車両が動いているか静止しているかを判定するのに用いられる。オドメータが、５回の連続したフィルタ更新に対してしきい値以下に変わる場合は、車両は停止していると考えられる。反対に、オドメータの計測値がいかなるフィルタ更新においてもしきい値よりも大きい値に変化する場合には、車両は動いていると考えられる。本分野の当業者であれば、他の方法を車両が静止しているか、又は動いているかを判定するのに用いてもよいことがわかる。例えば、カルマンフィルタから得られた車両速さの推定値は、しきい値に比較されたり、または異なる数の連続した更新を用いることができる。
好ましい実施例において、移動カルマンフィルタモデルから静止カルマンフィルタを用いることに変更することは、移動カルマンフィルタの最後の更新から得られた値（例えば、移動カルマンフィルタを用いるフィルタ）で静止カルマンフィルタ（例えば、静止カルマンフィルタモデルを用いるフィルタ）を初期化することによって達成される。例えば、静止カルマンフィルタにおける状態の初期値は、移動カルマンフィルタによって計算された最後の推定値である。先のフィルタの最後の推定値は、新しいフィルタを開始するのに最良の推定である。同様の推理が誤差共分散マトリックスに用いられる。ここで、状態の推定値の共分散を表す、移動カルマンフィルタの主対角線に沿った限界点は、静止カルマンフィルタの対角線共分散マトリックスを構成するのに用いられる。残ったマトリックスの値は、単位が減少するということを除いては、同じである。静止カルマンフィルタモデルから移動カルマンフィルタモデルに切り替えるとき、同じ処理が逆に用いられる。静止カルマンフィルタの共分散マトリックスの対角線の限界点は移動カルマンフィルタの共分散マトリックスの対角線を初期化するのに用いられる。残った５個の対角線限界点の値は、共分散マトリックスの初期値に対して本分野において公知である方法と類似した手段で選択される。
前述の記載は、図４に図示されており、段階３１４の処理をより詳細に表す。判定段階４０２において、ＴＲＵプロセッサ２０８は車両が動いているかどうかを判定する。好ましい実施例において、ＴＲＵプロセッサは、図３に示した処理ループを通して少なくとも５回の、しきい値以下であるオドメータ内における変化を捜す。ＴＲＵプロセッサ２０８が、車両が動いていないことを判定する場合には、ＴＲＵプロセッサはカルマンフィルタが判定段階４０４において初期化される必要あるかどうかを判定する。ＴＲＵプロセッサ２０８が、段階４０２の前に車両状態が移動状態から非移動状態に変更したことを判定する場合に、カルマンフィルタを初期化しなければならない。カルマンフィルタが初期化されるべきである場合には、ＴＲＵプロセッセ２０８は、上述したように、段階４０６において静止カルマンフィルタを初期化する。次に、段階４０８において、ＴＲＵプロセッサ２０８は、ＧＰＳ受信機測定値のみを用いて、カルマンフィルタを更新する。段階４１０において、ＴＲＵプロセッサ２０８は、更新されたカルマンフィルタを用いて車両の作動点を推定する。
ＴＲＵプロセッサ２０８が、車両が動くことを判定すると、ＴＲＵプロセッサは、カルマンフィルタが判定段階４１２において初期化される必要があるかを判定する。ＴＲＵプロセッサ２０８が、段階４０２の前に車両状態が非移動から移動段階に変わったことを判定した場合には、カルマンフィルタは初期化されなければならない。カルマンフィルタが初期化される必要がある場合には、ＴＲＵプロセッサ２０８は、上述したように段階４１４において移動カルマンフィルタを初期化する。次に段階４１６において、ＴＲＵプロセッサ２０８は、ＧＰＳ受信機測定値と、オドメータ２３０、ドプラ対地速度インジケータ２３２、向き変更率センサー２３４、及びかじ取りリゾルバ２３６から受け取られた測定値の双方を用いてカルマンフィルタを更新する。段階４１０において、ＴＲＵプロセッサ２０８は更新されたカルマンフィルタを用いて車両の作動点を推定する。
本発明は、いくつかの好ましい実施例を参照して部分的に示し、説明してきたが、本発明の当業者であれば、添付の請求の範囲に定義されているように本発明の精神と範囲から逸脱することなく、形態と詳細において様々な変更を行なえることがわかる。
【図１】
0003837160.tif 000002
【図２】
0003837160.tif 000003
【図３】
0003837160.tif 000004
【図４】
0003837160.tif 000005