(57)【特許請求の範囲】
【請求項１】
ビークルが所定のルートを走行することができるようにするシステムにおける該ビークルが追尾する経路を生成するシステムであって、
前記所定のルートに沿ったビークルの位置とヘッディングと曲率を各々が含む該ビークルの複数のポスチュアを表し該所定のルートを形成する複数の経路セグメントと、該所定のルートの始まりと終り及び隣接する経路セグメント間において前記所定のルートに沿って位置する複数のノードと、を示す前記所定のルートのためのルートデータを記憶する手段と、
前記複数の経路セグメントと前記複数のノードとを、それらをポスチュア連続の関数として表す圧縮された経路データとして記憶し、該圧縮経路データが前記所定のルートに要求される記憶量を減少させるようにする手段と、
前記所定のルートに関連する圧縮経路データを取り出す手段と、
ビークルが追尾するために前記取り出された圧縮経路データから一連のポスチュアを生成して該ビークルが前記所定のルートを走行することができるようにする手段と、
からなることを特徴とするシステム。
【請求項２】
ビークルが所定のルートを走行することができるようにするシステムにおける該ビークルが追尾する経路を生成する方法であって、
前記所定のルートに沿ったビークルの位置とヘッディングと曲率を各々が含む該ビークルの複数のポスチュアを表し該所定のルートを形成する複数の経路セグメントと、該所定のルートの始まりと終り及び隣接する経路セグメント間において前記所定のルートに沿って位置する複数のノードと、を示す前記所定のルートのためのル一トデータを記憶する段階と、
前記複数の経路セグメントと前記複数のノードとを、それらをポスチュア連続の関数として表す圧縮された経路データとして記憶して、該圧縮経路データが前記所定のルートに要求される記憶量を減少させるようにする段階と、
前記所定のルートに関連する圧縮経路データを取り出す段階と、
ビークルが追尾するために前記取り出された圧縮経路データから一連のポスチュアを生成して該ビークルが前記所定のルートを走行することができるようにする段階と、
からなることを特徴とする方法。
【請求項３】
請求項２に記載した方法であって、前記記憶する段階は、
前記複数の経路セグメントと前記複数のノードをクロソイド曲線として表す段階を含むことを特徴とする方法。
【請求項４】
請求項２に記載した方法であって、前記記憶する段階は、
前記複数の経路セグメントと前記複数のノードをBスプラインとして表す段階を含むことを特徴とする方法。
【請求項５】
請求項４に記載した方法であって、前記Bスプラインとして表す段階は、
ポスチュアのシーケンスを前記複数の経路セグメントと前記複数のノードから選定し、
十分な次数を有するBスプライン関数を前記シーケンスに適合させ、該Bスプライン関数が連続的であり、前記所定のルートの一部を表すようにする、
段階を含むことを特徴とする方法。
【請求項６】
請求項４に記載した方法であって、前記Bスプラインとして表す段階は、
ポスチュアのシーケンスを所定経路全体として選定し、
十分な次数を有するBスプライン関数を前記シーケンスに適合させ、該Bスプライン関数が連続的であり、前記所定のルートの全体を表すようにする、
段階を含むことを特徴とする方法。
【請求項７】
請求項４に記載した方法であって、前記記憶する段階は、
前記複数の経路セグメントの各々を個別のBスプライン関数として記憶し、
Bスプライン関数全体を前記個別のBスプライン関数に適合させて前記圧縮経路データを形成する、
段階を含むことを特徴とする方法。
【発明の詳細な説明】
【０００１】
発明の背景
１．発明の分野
本発明は位置特定システムに関し、より具体的には惑星の地表上にある、また、は地表付近にある自律ビークルの地上位置を決定するための位置特定装置及び方法に関する
２．関連技術
現在、アメリカ合衆国（Ｕ．Ｓ．）を含む若干の国の政府が、包括的にグローバル位置特定システム（ＧＰＳ）と呼ばれる地上位置決定システムを開発中である。ＧＰＳでは複数の衛星が惑星地球の周囲の軌道上に配置される。ＧＰＳ衛星は電磁信号を送信するように設計されている。これらの電磁信号から地表の、または地表付近のどの受信機も絶対地上位置（地球の中心に対する位置）を最終的に決定することができる。
Ｕ．Ｓ．政府はそのＧＰＳを“ＮＡＶＳＴＡＲ”と命名した。Ｕ．Ｓ．政府はＮＡＶＳＴＡＲ  ＧＰＳの運用は１９９３年になるであろうと宣言している。更にソビエト社会主義共和国連邦（Ｕ．Ｓ．Ｓ．Ｒ．）政府は、実質的にＮＡＶＳＴＡＲ  ＧＰＳと同一の“ＧＬＯＮＡＳＳ”を現在開発中である    ＮＡＶＳＴＡＲ  ＧＰＳでは、任意の時点に、６つの各軌道内に４個のＧＰＳ衛星が存在し、２１個の周回ＧＰＳ衛星を機能させ、３個のＧＰＳ衛星を予備にするという構想である。３つのＧＰＳ衛星軌道は、地球に対して相互に直交する面を有している。ＧＰＳ衛星軌道は、極軌道でも赤道軌道でもない。更に、ＧＰＳ衛星は１２時間で１回地球を周回する。
ＮＡＶＳＴＡＲ  ＧＰＳを使用すると、任意の地球上の受信機に対する周回中のＧＰＳ衛星の相対位置を電磁信号から決定することができる。この相対位置は一般に“擬似距離”と呼ばれている。更に、この相対位置は２つの方法によって計算することができる。
１つの方法は、発射される電磁信号の送信と受信との間の伝播時間遅延を測定することである。ＮＡＶＳＴＡＲ  ＧＰＳでは、電磁信号は、信号がＧＰＳ衛星から送信され
る時刻で連続的に符号化される。時間遅延を導出するために受信時刻に注目し、符号化された送信時刻を減算することが可能であるのは説明の要はあるまい。計算された時間遅延と、大気中の電磁波の走行速度の知識とから、擬似距離を正確に導出することができる。上述の方法を使用して計算された擬似距離を、本明細書では“実際の”擬似距離と呼ぶ。
別の方法は、周回衛星から送信される電磁信号内に符号化される衛星位置データを含む。ＮＡＶＳＴＡＲ  ＧＰＳの衛星位置データに関する暦データは公的に入手できる。電磁信号内に符号化されているデータに関してこの暦データを参照することによって擬似距離を正確に導出することができる、上述の方法を使用して計算された擬似距離を、本明細書では“推定”擬似距離と呼ぶ。
しかし、推定擬似距離を導出する従来方法において、衛星位置データがＧＰＳ衛星において１時間に１回毎正時だけにしか更新されないことに注意すべきである。従って更新された衛星位置データを使用して新しい推定擬似距離を計算する場合の推定擬似距離の精度は、次の正時まで各正時からの時間と共に低下する。
【０００２】
更に、少なくとも３個の周回ＧＰＳ衛星の相対位置を知ることによって、任意の地球受信機の絶対地上位置（即ち、経度、緯度、及び地球の中心に対する高度）を三角法を含む簡単な幾何学理論によって計算することができる。地上位置推定の精度は、部分的に、サンプルされる周回ＧＰＳ衛星の数に依存する。より多くのＧＰＳ衛星を計算に使用すれば、地上位置推定の精度を高めることができる。
普通は、地球受信機と種々のＧＰＳ衛星との間の回路時計の差がもたらす誤差ゆえに、４個のＧＰＳ衛星をサンプルして各地上位置推定を決定する。時計差は数ミリ秒であり得る。もし地球受信機の時計がＧＰＳ衛星の時計と同期していれば、地球受信機の位置を正確に決定するためには３個のＧＰＳ衛星をサンプルすれば十分であろう。
ＮＡＶＳＴＡＲ  ＧＰＳでは、電磁信号は全てのＧＰＳ衛星から単一の搬送波周波数で連続的に送信される、しかし各ＧＰＳ衛星は異なる変調計画を有しており、それによって信号を区別することができる。ＮＡＶＳＴＡＲ  ＧＰＳでは、搬送波周波数は各ＧＰＳ衛星に独特な擬似ランダム信号を使用して変調される。従って、搬送波周波数を解号すればＮＡＶＳＴＡＲ  ＧＰＳ内の周回ＧＰＳ衛星を識別することができる。
更に、ＮＡＶＳＴＡＲ  ＧＰＳは、擬似ランダム数（ＰＲＮ）信号を使用する２つの搬送波変調モードを考えている。“粗／収集”（Ｃ／Ａ）モードと呼ばれる一方のモードでは、ＰＲＮ信号は１．０２３ＭＨｚのチップレートを有するゴールドコードシーケンスである。ゴールドコードシーケンスは公知の普通の擬似ランダムシーケンスである。チップはこの擬似ランダムコードの１つの個々のパルスである。擬似ランダムシーケンスのチップレートはシーケンス内でチップが生成されるレートである。従って、チップレートはコード線り返しレートをそのコード内のメンバーの数で除したものに等しい。従って、ＮＡＶＳＴＡＲ  ＧＰＳの“粗／収集”モードでは、各ゴールドコードシーケンス内には１，０２３チップが存在し、シーケンスは１ミリ秒に１回繰り返される。４個の周回ＧＰＳ衛星からの１．０２３ＭＨｚのゴールドコードシーケンスを使用することによって、地球受信機の地上位置をほぼ６０乃至３００メートルの精度で決定することができる。ＮＡＶＳＴＡＲ  ＧＰＳにおける第２の変調モードは一般に“精密”または“保護”（Ｐ）モードと呼ばれるものである。Ｐモードでは擬似ランダムコードは１０．２３ＭＨｚのチップレートを有している。更にＰモードシーケンスは極めて長く、シーケンスは２６７日に１回しか繰り返されない。その結果、任意の地球受信機の地上位置をほぼ１６乃至３０メートルの精度で決定することができる。
【０００３】
しかしながらＰモードシーケンスは機密扱いされ、合衆国政府から公式に入手することはできない。換言すれば、Ｐモードは合衆国政府によって承認された地球受信機だけが使用するようになっているのである。
異なる周回ＧＰＳ衛星からの種々のＣ／Ａ信号を地球受信機が区別するために、地球受信機は通常、ゴールドコードシーケンスを局部的に生成するための複数の異なるゴールドコード源を含んでいる。局部的に導出されるゴールドコードシーケンスは、各ＧＰＳ衛星からの独特なゴールドコードシーケンスに対応する。
局部的に導出された各ゴールドコードシーケンスと送信されたゴールドコードシーケンスとは、１ミリ秒のゴールドコードシーケンス周期にわたって互いに相互相関される。局部的に導出されるゴールドコードシーケンスの位相は、最大の相互相関関数が得られるまでチップ毎に、次いでチップ内で変化する。１，０２３ビットの長さを有する２つのゴールドコードシーケンスの相互相関は、他の何れの組合わせのゴールドコードシーケンスの相互相関関数よりも約１６倍も大きいから、局部的に導出されるゴールドコードシーケンスをＧＰＳ衛星の１つが送信した同一ゴールドコードシーケンスにロックさせることは比較的容易である。
【０００４】
地球受信機の視野内の少なくとも４個のＧＰＳ衛星からのゴールドコードシーケンスは、このようにして、局部的に導出された各ゴールドコードシーケンスに順次に応答する単一の信号チャネルを使用することによって、または代替として、異なるゴールドコードシーケンスに同時に応答する並列チャネルを使用することによって分離される。４つの局部的に導出されたゴールドコードシーケンスを地
球受信機の視野内の４個のＧＰＳ衛星から受信したゴールドコードシーケンスに同相にロックさせると、地球受信機の相対位置を約６０乃至３００メートルの精度で決定できる。
上述のＮＡＶＳＴＡＲ  ＧＰＳの近似推定は（１）地球受信機が実際に応答している信号を送信中のＧＰＳ衛星の数、（２）受信信号の可変振幅、及び（３）異なるＧＰＳ衛星からの受信信号間の相互相関ピークの大きさによって影響を受ける。
【０００５】
地球受信機は同時に複数のＰＲＮ信号を受信するから、若干のコードが対立す
る共通時間間隔が存在する。換言すれば、対立する受信信号間の相互相関のために受信した各ＰＲＮの到着時刻の測定をコードが劣化させるのである。
各ＰＲＮ毎の到着時刻の測定は、受信したＰＲＮ信号のゴールドコードシーケンスと局部的に導出したＰＲＮ信号のゴールドコードシーケンスとの間の相互相関のピーク振幅の時刻の決定によって行われる。局部的に導出したＰＲＮ信号が受信したＰＲＮ信号上に重畳されると、それによってそれらの相互相関の平均時間が増加し、平均雑音寄与が減少する、しかし、受信したＰＲＮ信号間の相互相関誤差が周期的であるために、平均時間の増加は、誤差信号及び受信したＰＲＮ間の相互相関値の両者に同様な増加をもたらす。従って、ＰＲＮ信号の到着時刻に関連する誤差が相互相関によって減少することはない。
ＧＰＳに加えて、ビークルの位置推定を求めるために航法システムに慣性システムを使用することが従来から知られている、慣性参照ユニット（ＩＲＵ）は、ジャイロスコープまたはジャイロによって安定化された参照座標フレーム内の加速度計から比力測定を入手する、ＩＲＵは、例えばレーザ、機械式、または光ファイバを含む幾つかの型であることができる。ＩＲＵを使用する無支援航法システムでは加速度計によって測定された比力（地球の重力の効果に対して補正されている）は航法数式内に統合され、ビークルの位置及び速度が求められる。
ＩＲＵの計器測定は、プラットフォーム装置に依存して、参照座標フレームではなく異なる直角座標フレーム内に指定することができる。近地球航法に最も一般的に使用されている参照航法は局部水準フレーム（東・北・垂直）である・幾つかのジンバル付きプラットフォーム装置が上述の参照航法フレームと共に存在している。
【０００６】
ジンバル付き局部水準・北探索ＩＲＵでは、ジャイロスコープ及び加速度計は
プラットフォーム上に取り付けられ、このプラットフォームにはその水準及び北を指す方位を維持するようなトルクが与えられている。このプラットフォームが参照面である、これに対して、ジンバル付き局部水準・方位漂動ＩＲＵではプラットフォームは水平に保たれるが、垂直軸を中心とするトルクは与えられない。
更に、ストラップダウンＩＲＵでは、ジャイロスコープ及び加速度計はビークル本体に直接取り付けられる。これらは慣性空間に対するビークルの線形及び角度運動を測定する、この運動はビークル座標で表される。従って、ストラップダウンＩＲＵでは、参照航法フレームに対する高度を先ず計算する必要がある。次いで計算された高度を使用して加速度計測定を参照フレームに変換する。ストラップダウンＩＲＵの加速度計データを参照フレームに補外した後の前述の航法式の解はジンバル付きＩＲＵ及びストラップダウンＩＲＵの両者において同一である。
ストラップダウンＩＲＵでは、加速度計測定を分解するために要求される高度計算は通常高速で遂行される。これらの計算は、コンピュータのバイトサイズ及び使用可能な処理能力に限界があることから数値誤差を生じ易い。これらの計算誤差はセンサループ、データ転送速度、及びサンプリング時点におけるセンサ出力の分解能及び大きさに依存する。
しかし、重要な便益はジンバル付きＩＲＵを使用してではなく、ストラップダウンＩＲＵを使用してもたらされる。ストラップダウンＩＲＵはより安価である。更にストラップダウンＩＲＵは一般的に物理寸法が小さい。従ってＩＲＵは、その寸法及び価格の節約を実現する潜在能力から、ストラップダウンＩＲＵが軍用及び商用の両応用にとって魅力的である。
ＩＲＵを使用する航法システムの性能は主としてＩＲＵ内の種々の構成センサが関与する誤差によって制限される。ジャイロスコープは漂動する。加速度計は本質的にバイアスされている。更に不適切な倍率及び不適切なＩＲＵ整列角が誤差に関与する。典型的にはそれまでの誤差が、ビークルの任務の進行につれて時間と共に累積されるビークルの位置、速度、及び高度の推定に不正確さをもたらす。誤差はある程度まではユーザダイナミックスに依存する。
もしあるビークルが極めて正確な航法システムを要求すれば、その要求を満足するために高精度ジャイロスコープ及び加速度計を使用することは可能である。しかし、このような高精度機器はビークルの複雑さと価格とを増大させる。
自律ビークル航法も公知である。“自律”とは人手によらない、または機械制御されることを意味する、しかしながら、従来の自律システムはよく見ても初歩的である。
【０００７】
視覚検知に基づく位置特定に頼る自律システムが存在している。例えば、視覚をべ一スとする位置特定がＩＥＥＥ，ＣＨ２５５５−１／８８／００００／０９１２＄０１．００．１９８８に所載のＲ．テリー・ダンレーの論文“自律陸上ビークルのための障害物回避認知処理”に記載のマーチンマリエッタ自律陸上ビークル内に使用されている。
視覚をべ一スとする若干の位置特定システムは、点から点へ航行するために例えば工場の床上の固定案内線またはマークを使用手る。他の位置特定システムは複雑なハードウエア及びソフトウエアによるパターン認識を含む。“非天測位置推測法”と呼ばれる更に他のシステムでは、既知の出発点に対するビークルの位置を追尾することによって航行する。この追尾は、ビークルが走行した距離を測定し、出発点からのビークルの方向を監視することによって遂行される。従来の自律航法システムは種々の欠点及び制限を受け易い。例えば、もしビークル上の航法システムがそのビークルが何処に位置しているのかを認識し損なうか、またはビークルが辿った足跡を見失うか、またはビークルの出発点を誤計算すれば、航法システムはビークルをその最終行先へ到達させるように正確に導くことができなくなる。
更に、従来の自律航法システムでは、ビークルの位置推定の誤差が時間と共に累積される傾向を有しており、航法システムは時間を費やす初期化を屡々行う必要がある、最後に、従来の航法システムはビークルの経路に沿ってパターンまたはマーカーを配置する必要がある。このパターンまたはマーカーの配置も時間を費やし、高価であり、小さい制御された領域へのこれらの航法システムの適用能力を制限する。
【０００８】
発明の概要
本発明はビークル位置特定システムであり、明細書全体を通して使用されているシステムとは、装置、方法、または装置及び方法の両者の組合せを意味している。本発明は、従来のビークル位置決定の技術の多くの限界を打破する。
本発明は、自律ビークルのための如何なる航法システムをも支援するために使用可能である。自律ビークルは静止していてもまたは移動していても差し支えない。更に、自律ビークルは地表にあっても、または地表付近にあっても差し支えない。換言すれば、本発明は如何なる地上ビークルをも極めて正確且つ高速に追尾する。
本発明は、ＩＲＵ及びＧＰＳの従来の能力を、費用に対して最も効率的に組合せて大幅に向上させ、地上ビークルの極めて正確な位置推定を提供することを意図している。そのようにするに当たって本発明は、優れた位置特定能力を、従って柔軟な自律航法能力を考慮に入れた装置及び方法を含む多くの新しく且つ創意に富むシステムを使用する。
【０００９】
本発明は更に、地表の、または地表付近のビークルの位置推定を決定するために３つの独立したサブシステムの新しく且つ創意に富む組合せをも含む。１つのサブシステムは、例えばＮＡＶＳＴＡＲ  ＧＰＳのようにＧＰＳを使用する第１位置特定システムである。この第１位置特定システムはビークルの第１位置推定を計算する。
別のサブシステムは、ＩＲＵ及びビークルの走行距離計を使用する第２位置特定システムである。この第２位置特定システムは第２位置推定を計算する。最後のサブシステムは、上記２つのサブシステムからの第１及び第２位置推定に基づいてより正確なビークルの第３位置推定を計算する処理システムである。
本発明は星座効果方法を含む。星座効果方法は、ビークルの視野内のＧＰＳの大きい群から最適衛星星座を選択し、ＧＰＳから導出する第１位置推定の精度を向上させる。
本発明は、ＧＰＳ及びＩＲＵの両者または何れか一方から得た位置特定用データ内の雑音及び誤差を補償する差動修正技術／方法を提供することによって、ビークル位置推定の精度を高める。好ましい実施例では、参照点として働くベースステーションが差動修正技術／方法を遂行することができ、また得られたデータをビークルヘ中継することができる、ビークルはべ一スステーションから受信したデータを使用してビークルの位置推定の精度を高めることができる。
本発明は、ＧＰＳ衛星から受信したＧＰＳデータの精度を高めるために、放物線バイアス技術を含む。放物線バイアスは各ＧＰＳ衛星毎に導出され、そのＧＰＳ衛星のための実際の擬似距離を向上させる。放物線バイアス技術では、放物線モデルが実際の擬似距離に関して作成され、この放物線モデルから放物線バイアスが補外される。
本発明は、ＧＰＳ衛星から受信したＧＰＳデータの精度を高めるために、べ一ス残留バイアス技術を含む。べ一ス残留バイアスは、ビークル上のＶＰＳからの第１位置推定を変更するために導出される。べ一ス残留バイアスは、べ一スステーションの既知の位置とその推定された位置との実効的な差である空間的なバイアスである。
【００１０】
本発明は、新しい衛星位置予測方法を含む。この方法によって、本発明はＧＰＳ衛星の将来位置を予測することができる。その結果、位置特定システムの精度及び性能が更に高められる。
本発明は、ＧＰＳから最終的に導出される第１位置推定の精度を向上させるために重み付き経路履歴技術を含む。重み付き経路履歴技術は、先行第１位置推定を使用してビークル経路モデルを導出し、このモデルによって将来の第１位置推定の有効性を試験する。重み付き経路履歴技術を使用することによって第１位置推定の漂動が減少し、偽の位置計算を行わない特性が向上する。
本発明は更に、ＧＰＳの任意のＧＰＳ衛星から受信したデータの反選択可用性をも提供する。反選択可用性技術は任意のＧＰＳから受信した偽の位置特定用データを検出し、修正する。（１）Ｕ．Ｓ．及びＵ．Ｓ．Ｓ．Ｒ．のそれぞれの政府によって故意に改悪されたり、または（２）技術的な誤動作のために、ＮＡＶＳＴＡＲ  ＧＰＳまたはＧＬＯＮ  ＡＳＳ  ＧＰＳから偽のデータを受信することがあり得る。
当業者ならば添付図面を参照しての以下の詳細な説明から、本発明のさらなる特色及び長所が明白になるであろう、説明には何等かの付加的な特色及び長所も含まれていよう。
【００１１】
請求の範囲に限定されている本発明は本文及び以下の図面を参照すると理解し易いであろう。
図1は本発明の好ましい実施例の高水準ブロック線図100を示し、
図1Aは6つの軌道面174-184内に配分された21個の機能GPS衛星130-170と、3個の予備GPS衛星(図示してない)とからなるNAVSTAR GPS内の機能GPS衛星の高水準ブロック線図100Aであり、
図2はNAVSTAR GPSの4個のGPS衛星に関する4つの連立航法方程式を示す図であって、これらの方程式はGPS衛星200-206とビークル102との間の時計バイアスCbを含み、
図3は本発明を実現し、実施できる典型的な自律作業現場の高水準ブロック線図300であり、
図4は本発明のナビゲータ406、ビークルVPSアーキテクチャ1000、及びビークル制御装置408の間の相互関係の高水準ブロック線図400であり、
図5は本発明による自律制御システム内の種々の要素の脈絡と、それらの相互関係とを示す高水準ブロック線図500であり、
図6はビークル102の位置を正確に決定するために、GPS衛星星座200、202、204及び206を含み、また擬似衛星105及びべ一スステーション188と共に使用される、適切にはNAVSTAR GPSであるGPSの動作の高水準ブロック線図600であり、
図7は好ましい実施例のGPS処理システムの電気的アーキテクチャ/ハードウエア700を示す低水準ブロック線図であり、
図8は好ましい実施例の図7に示すようなGPS処理システム700におけるソフトウエアの機能を示す低水準流れ図800であり、
図9は好ましい実施例では走行距離計902及び慣性参照ユニット(IRU)904を含むMPSのアーキテクチャ/ハードウエア900を示す中間水準ブロック線図であり、図10は好ましい実施例におけるVPSのVPSアーキテクチャ/ハードウエア1000を示す中間水準ブロック線図であり、図11は図10のVPSアーキテクチャ1000の低水準ブロック線図1100であり、
図12は図10のVPS主(I/O)プロセッサ1O02の中間水準ブロック線図1200であって、VPSカルマンフィルタ1202及び重み付きコンバイナ1200を示し、
図12Aは好ましい実施例のスーパーカルマンフィルタ1200Aの高水準ブロック線図であり、
図13は本発明の好ましい実施例において第1位置推定の精度を改善するための星座効果方法の流れ図1300であり、
図14は4つのGPS衛星(図示してない)のGPS衛星星座に関する一組の計算された推定擬似距離1404、1406、1408、及び1410を示す座標系1402上の極プロット1400であって、陰影領域1412は擬似距離1406及び1408を生じさせたGPS衛星を顧慮した時のビークルの考え得る位置推定を示し、
図15は好ましい実施例の原形バイアス技術の流れ図1500であり、
図16は好ましい実施例の放物線バイアス技術の流れ図1600であり、
図17は好ましい実施例のべ一ス残留バイアス技術の流れ図1700であり、
図17Aは、好ましい実施例のべ一ス相関器バイアス技術の流れ図1700Aであり、  図18は将来衛星位置の予測のための好ましい実施例における方法の流れ図1800であり、
図19は本発明の重み付き経路履歴技術の流れ図1900であり、
図20はビークル102の第1位置推定の高水準図形表示2000であって図19に示す重み付き経路履歴方法は第1位置推定2010がビークル経路と全く一致しないためにそれを排除するであろうことを示し、
図20Aは図19及び図20に示す重み付き経路履歴技術を実現するための方法の高水準流れ図2000Aであり、
図21は本発明の反選択可用性技術の流れ図であり、
図22は本発明によるノード及びセグメントを使用するビークルルート限定の図2200であり、
図23はポスチュア及び関係付けられた円が如何にして目標点から求められるかを図式的に示す図2300であり、
図24は最初のクロソイドセグメントの符号を如何にして決定するかを示す図2400であり、
図25は最後のクロソイドセグメントの符号を如何にして決定するかを示す図2500であり、
図26はクロソイド曲線を図式的に示す図2600であり、
図27は近似フレネル積分を計算するための数値的方法の流れ図2700であり、
図28は経路の再計画を示す図2800であり、
図29は二次、三次、及び四次のBスプライン曲線の図2900であり、
図30は本発明のポスチュア・リングバッファの実施例を示す図3000であり、
図31は本発明の好ましい実施例の経路追尾制御アーキテクチャ/ハードウエアの高水準ブロック線図3100であり、
図32は舵取り計画サイクルにおける関連ポスチュアを示す図3200であり、
図33は曲率を含む誤差ベクトルを如何に計算するかを示す図3400であり、
図34は曲率を含む誤差ベクトルを、包含されたビークル経路を用いて如何に計算するかを示す図3400であり、
図35は本発明のナビゲータ406の脈絡図3500であり、
図36は本発明の経路追尾構造の脈絡図3600であり、
図37A-37Dはそれぞれナビゲータ406のデータ流概要3700A-3700Dであり、
図38Aはビークル搭載スキャナ404の図3800Bであり、
図38Bは障害物4002に対する自律ビークル走査102を示す図3800Bであり、
図39は本発明のレーザ走査システムにおける選択された走査線3904及び3906の図3900であり、
図40は障害物4002を回避する自律ビークル102を示す図4000であり、
図41は本発明の好ましい実施例による障害物処理の図4100であり、
図42は本発明の好ましい実施例における障害物検出に使用されるレーザスキャナシステムの中間水準ブロック線図4200であり、
図43は本発明の自律採鉱用ビークルのための制御システムの中間水準ブロック線図4300であり、
図44は図43の制御システムの動作モード間の遷移を示す状態図4400であり、
図45は好ましい実施例の遠視線遠隔制御システムの高水準ブロック線図4500であり、
図46は好ましい実施例の速度制御4304の高水準ブロック線図4600であり、
図47は好ましい実施例の速度制御4304のサービスブレーキ制御回路の高水準ブロック線図4700であり、
図48は好ましい実施例の速度制御4304のガバナ制御回路の高水準ブロック線図4800であり、
図49は本発明の好ましい実施例の舵取り制御システムの舵取り制御回路4306の高水準ブロック線図4900であり、
図50は本発明の速度制御430の駐車ブレーキ制御回路の高水準ブロック線図5000であり、
図51は本発明の航法システムを開発するために使用される三輪車舵取りモデルの高水準ブロック線図5100であり、
図52は本発明の遮断回路の実施例を示す中間水準ブロック線図5200であり、
図53はナビゲータ406のタスクを示す低水準通信図5300であり、
図54はナビゲータ406のナビゲータ共有メモリの実施例を示す中間水準通信図5400であり、
図55は実行判断に関する高水準監視流れ図5500を示し、
図56は図56A-56Dの実行流れ図5600A-5600Dの関係/接続の中間水準流れ図5600であり、
図56A-56Dはそれぞれ、高水準流れ図5500の低水準実行流れ図5600A-5600Dであり、
図57A-57Rはそれぞれ、各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
図58は図58A-58Cの相互関係の高水準流れ図5800であり、そして、
図58A-58Cはそれぞれ、各実行流れ図5700A-5700Qにおける”状態に基づく動作"を示す低水準流れ図5800A-5800Cである。
【００１２】
0003560959.tif 000002
0003560959.tif 000003
0003560959.tif 000004
0003560959.tif 000005
【００１３】
I．定義
以下のアルファベット順の定義リストは、本発明を理解し易くするためのものである。
(1)“絶対位置"(Absolute position)本明細書においては、地球の中心に対する位置のことである、一般的に絶対位置は、何れも地表または地表付近にあるビークルまたはべ一スステーションに関連していよう。本発明の好ましい実施例においては、第1、第2、及び第3位置推定は全て絶対位置である。
(2)“実際の擬似距離"(Actual pseudorange)とは、(1)参照点と(2)地上位置決定システムの源との間の距離の近似を意味する。本明細書では実際の擬似距離とは、通常、(1)地球受信機と(2)GPS衛星及び擬似衛星の両方または何れか一方との間の距離のことを言う。実際の擬似距離は、GPS衛星及び擬似衛星の両方または何れか一方から発せられる電磁信号の送信と受信との間の伝播時間遅延を最初に測定することによって近似される。実際の擬似距離は、計算された時間遅延に光速、即ち2.9979245898*108m/sを乗ずることによって容易に計算できる。
(3)“反選択可用性"(Anti-selective availability)とは、変調の粗/収集(C/A)モードにおける改悪されたGPSデータを検出し、補償するための方法/技術/処理過程のことである。
(4)“自律"(Autonomous)は、本明細書においては、その普通の意味で使用される。これは、完全に自動的であるか、または実質的に自動的であるか、または動作に殆ど人間が関与しないかの何れかであるような動作を言う。一般に自律ビークルとは、運転に人手を要さないビークルか、または操縦士もしくは副操縦士を必要とせずに動作するビークルを意味する。しかしながら、自律ビークルは自動的に駆動または他の方法で作動せしめられてもよく、1人または複数の乗客を乗せていてもよい。
(5)“べ一ス相関器バイアス"(Base correlator bias)は図17Aの流れ図1700Aに従って導出される空間バイアスを意味する。
(6)“べ一ス相関器バイアス技術"(Base correlator bias technique)は、べ一ス相関器バイアスを計算するための方法/処理過程を意味する。
(7)“べ一ス推定位置"(Base estimated position)または“BEP"は、ビークルに対するべ一スステーションの相対位置のことである。BEPは、本明細書の11.F.2.d.rべ一ス相関器バイアス技術」において使用される。
(8)“べ一ス既知位置"(Base known position)または“BKP"は、既知のべ一スステーション(参照点として使用される)の絶対位置である。BKPは何等かの正確な位置特定システムから導出されたそれ自身の推定であることができる。BKPは、何等かの他の位置推定よりも正確なべ一スステーションの絶対位置であると見做される。
(9)“べ一ス位置推定"(Base position estimate)は、ホスト処理システム内のGPS処理システムから導出されたべ一スステーションの絶対位置推定を意味する。べ一ス位置推定はビークル上のGPS処理システムによって導出された第1位置推定と実質的に同一である。べ一ス位置推定は、本明細書のII.F.2.c.「べ一ス残留バイアス技術」において計算される。
【００１４】
（１Ｏ）“べ一ス残留バイアス”（Ｂａｓｅ  ｒｅｓｉｄｕａ１ｓ  ｂｉａｓ）は、べ一スステーションのべ一ス既知位置（ＢＫＰ）とホスト処理システムによって計算されるべ一スステーションの位置推定との実効差である空間バイアスを意味する。
（１１）“べ一ス残留バイアス技術”（Ｂａｓｅ  ｒｅｓｉｄｕａｌｓ  ｂｉａｓ  ｔｅｃｈｎｉｑｕｅ）はべ一ス残留バイアスを導出する方法を言う。
（１２）“バイアス”（Ｂｉａｓ）とは、２つの測定の差、通常は位置推定の差（空間バイアス）、またはクロックレートの差（時計バイアス）のことである。通常、一方の測定の方が別の測定より正確であることが知られているから、バイアスは“誤差”と呼ばれることが多い。
（１３）“時計バイアス”（Ｃ１ｏｃｋ  ｂｉａｓ）とは、（１）ＧＰＳ衛星及び擬似衛星の両者または何れか一方の送信回路と（２）地球受信機の受信回路との間の時計時刻の差を意味する。空間バイアスの計算において時計バイアスを使用する場合、時計バイアスには光速、即ち２．９９８＊１０８ｍ／ｓが乗ぜられる。従って、時計バイアスは長さの単位に変換されるのである。
（１４）“星座”（Ｃｏｎｓｔｅｌｌａｔｉｏｎ）は、地表の、または地表付近の点の絶対位置推定を導出するために使用される信号を発するＧＰＳ衛星及び擬似衛星の両者または何れか一方からなる群のことである。後述する“最適星座”を参照されたい。
（１５）“星座効果法”（Ｃｏｎｓｔｅｌｌａｔｉｏｎ  ｅｆｆｅｃｔ  ｍｅｔｈｏｄ）は、ビークルの視野内のＧＰＳ衛星の大きい群からＧＰＳ衛星の最適星座を選択する技術または処理過程を意味する。
（１６）“データ無線”（Ｄａｔａ  ｒａｄｉｏ）は、無線周波数（ＲＦ）において通信するための送信機、受信機、送受信機、またはそれらの任意組合せを言う。
（１７）“地球受信機”（Ｅａｒｔｈ  ｒｅｃｅｉｖｅｒ）は、ＧＰＳ衛星及び擬似衛星の両者または何れか一方から信号を受信し、処理する何等かの装置またはデバイス、またほそれらの何等かの部分のことである。地球受信機は地表に、または地表付近に位置決めすることができる。更に地球受信機は、例えばビークルまたはべ一スステーションの形状をとることができる。
（１８）“推定擬似距離”（Ｅｓｔｉｍａｔｅｄ  ｐｓｅｕｄｏｒａｎｇｅ）とは、（１）参照点と（２）地上位置決定システムの源との間の距離の近似のことである。本明細書では実際の擬似距離とは、通常（１）地球受信機と（２）ＧＰＳ衛星及び擬似衛星の両者または何れか一方との間の距離の近似のことである。推定擬似距離は、ＧＰＳ衛星及び擬似衛星の両者または何れか一方から送信される電磁信号上に符号化されているＧＰＳデータから計算される。ＮＡＶＳＴＡＲ  ＧＰＳのＧＰＳデータから推定擬似距離を計算するための公式を載せた暦を公的に入手することができる。
（１９）“第１位置推定”（Ｆｉｒｓｔ  ｐｏｓｉｔｉｏｎ  ｅｓｔｉｍａｔｅ）または“ＦＰＥ”または“ＦＰＥ（ｉ）”は、どのような形状であっても、ＧＰＳから出力される何れかのビークルの推定絶対位置のことである。本発明では、第１位置推定及び第２位置推定は独立的に導出される。爾後にこれらの推定は組合わされ、濾波されて第３の位置推定が導出される。従って、第１位置推定の精度は第３位置推定の精度に影響する。
【００１５】
（２０）“ＧＬＯＮＡＳＳ  ＧＰＳ”は、Ｕ．Ｓ．Ｓ．Ｒ。によって設計され、現在開発中のＧＰＳである。
（２１）グローバル位置特定システム”（Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  ｓｙｓｔｅｍ）または”ＧＰＳ”は、１つの型の地上位置決定システムである。ＧＰＳでは、複数の衛星が惑星地球を回る軌道内に配置される。ＧＰＳ衛星は電磁信号を送信するように設計されている。これらの電磁信号から、地表上の、または地表付近の如何なる受信機もこの絶対地上位置（地球の中心に対する位置）を最終的に決定することができる。Ｕ．Ｓ．政府は、そのＧＰＳを“ＮＡＶＳＴＡＲ”と命名した。Ｕ．Ｓ．Ｓ．Ｒ．政府は、そのＧＰＳを“ＧＬＯＮＡＳＳ”と命名している。
（２２）“ＧＰＳデータ”（ＧＰＳ  ｄａｔａ）とは、ＧＰＳのＧＰＳ衛星から送信される信号上に符号化されている全てのデータのことである。ＧＰＳデータは、例えば天体暦データ及び時刻データを含む。
（２３）”ＧＰＳ処理システム”（ＧＰＳ  ｐｒｏｃｅｓｓｉｎｇ  ｓｙｓｔｅｍ）は、地上位置決定システムから信号を受信し、受信した信号からビークルの第１位置推定を導出するための本発明のシステムである。好ましい実施例では、ＧＰＳ処理システムはＧＰＳのＧＰＳ衛星及び擬似衛星の両者または何れか一方から電磁信号を受信する。
（２４）“ホスト処理システム”（Ｈｏｓｔ  ｐｒｏｃｅｓｓｉｎｇ  ｓｙｓｔｅｍ）は、ビークルの位置推定の精度を高める方法及び技術を遂行するためにべ一スステーションにおいて作動するコンピュータシステムである。これらの方法及び技術から導出したデータはビークルヘ送信されるので、ビークルは第１、第２、及び第３位置推定を計算する際にこのデータを使用することができる。好ましい実施例では、ホスト処理システムのアーキテクチャ／ハードウエアはＶＰＳのアーキテクチャ／ハードウェアと実質的に同一である。
（２５）“慣性参照ユニット”（Ｉｎｅｒｔｉａｌ  ｒｅｆｅｒｅｎｃｅ  ｕｎｉｔ）または“ＩＲＵ”は通常はビークルに搭載され、ビークルの第２位置推定の導出を支援するシステムである。ＩＲＵは、ジャイロスコープまたはジャイロによって安定化されている参照座標フレーム内の加速度計から比力（ｓｐｅｃｉｆｉｃ−ｆｏｒｃｅ）測定を入手する。ＩＲＵは、レーザ型または機械型であって差し支えない。ＩＲＵを使用する無支援航法システムでは、加速度計によって測定された比力（地球重力の効果に関して補正済）は航法数式に統合されてビークルの位置及び速度を発生する。好ましい実施例では、ＩＲＵはＭＰＳの部分である。
（２６）”カルマンフィルタ”（Ｋａｌｍａｎ  ｆｉ１ｔｅｒ）は、その普通の意味で使用される。これは、データ内の雑音または誤りを濾波して排除するためのソフトウエアプログラムのことである。好ましい実施例では、第１位置推定の精度を高めるべく雑音または誤りを濾波して排除するためにＧＰＳ処理システムにおいてＧＰＳカルマンフィルタが使用されている。また第２位置推定の精度を高めるべく雑音または誤りを濾波して排除するためにＶＰＳにおいてＶＰＳカルマンフィルタが使用されている。
（２７）“移動体位置特定システム”（Ｍｏｔｉｏｎ  Ｐｏｓｉｔｉｏｎｉｎｇ  ｓｙｓｔｅｍ）または“ＭＰＳ”は、少なくともＩＲＵと、ビークル走行距離計とを備えている。好ましい実施例では、ＭＰＳは地表の、または地表付近の如何なるビークルの第２位置推定をも導出する。更に、べ一スステーションは本質的に静止しているから、べ一スステーションにＭＰＳは必要ではない。
（２８）“最適星座”（Ｏｐｔｉｍａｌ  ｃｏｎｓｔｅｌｌａｔｉｏｎ）は空間におけるＧＰＳ衛星の相対位置が、地表の、または地表付近の点の最も正確な推定を導出する優れた三角測量を提供するような衛星星座を意味する。
（２９）“原形バイアス”（Ｏｒｉｇｉｎａｌ  ｂｉａｓ）は、実際の擬似距離から、推定擬似距離と（長さの単位での）時計バイアスとを減算することによって計算される空間バイアスを意味する。時計バイアスは、それらに光速、即ち２．９９７９２４５８９８＊１Ｏ８ｍ／ｓを乗ずることによって長さの単位に変換されている。
【００１６】
（３０）“原形バイアス技術”（Ｏｒｉｇｉｎａｌ  ｂｉａｓ  ｔｅｃｈｎｉｑｕｅ）は、原形バイアスを計算する方法である。
（３１）“ＮＡＶＳＴＡＲ  ＧＰＳ”は、Ｕ．Ｓ．政府によって設計され、現在開発中のＧＰＳである。
（３２）”航法システム”（Ｎａｖｉｇａｔｉｏｎ  ｓｙｓｔｅｍ）は、地表の、または地表付近の如何なるビークルをも案内する何等かのシステム及び方法の両者または何れか一方を言う。航法システムはビークルに搭載できる。本発明のＶＰＳは、ビークルの航法システムに極めて正確な第３位置推定を供給できるので、それによって航法システムはビークルを精密に案内することができる。
（３３）“放物線バイアス”（Ｐａｒａｂｏｌｉｃ  ｂｉａｓ）は、各観測されたＧＰＳ衛星の実際の擬似距離に関する放物線モデルを作成し、これらの放物線モデルから複数の値を補外することによって計算される空間バイアスである。好ましい実施例では、放物線バイアスは、実際の擬似距離、マイナス作成された放物線モデルから補外した値、マイナス時計バイアス（光遠を乗ずることによって長さの単位）である。
（３４）“放物線バイアス技術”（Ｐａｒａｂｏｌｉｃ  ｂｉａｓ  ｔｅｃｈｎｉｑｕｅ）は、使用される各ＧＰＳ衛星に関して放物線バイアスを計算するための方法である。
（３５）“好ましい実施例”（Ｐｒｅｆｅｒｒｅｄ  ｅｍｂｏｄｉｍｅｎｔ）は、本発明を実現する最良モードのことである。好ましい実施例は単に例示に過ぎない。本発明は、好ましい実施例によって限定されるものと解釈すべきではない。
（３６）“擬似衛星”（Ｐｓｅｕｄｏｌｉｔｅ）はＧＰＳ衛星に似せた地表または地表付近の放射装置である、好ましい実施例では、ＧＰＳ衛星からの電磁信号に類似した電磁信号が陸上基地擬似衛星から送信される。ＧＰＳ衛星に似せるために１またはそれ以上の擬似衛星を使用して第１位置推定の計算を向上させることができる。
（３７）“擬似衛星データ”（Ｐｓｅｕｄｏｌｉｔｅ  ｄａｔａ）は、擬似衛星から受信する信号上に符号化されている全てのデータを意味する。擬似衛星データは多くの点でＧＰＳデータに類似し、同じような情報を含む。
（３８）“擬似距離”（Ｐｓｅｕｄｏｒａｎｇｅ）は、地上位置決定システムの源と地表または地表付近の点との間の距離である。好ましい実施例では、これらの源はＧＰＳ衛星及び擬似衛星の両者または何れか一方であることができる。地上位置決定システムは、擬似衛星（もし使用すれば）と共に使用されるＧＰＳであることができる。更に、地表または地表付近の点は、べ一スステーション及びビークルの両方または何れか一方であることができる。
（３９）“衛星位置予測法”（Ｓａｔｅｌｌｉｔｅ  ｐｏｓｉｔｉｏｎ  ｐｒｅｄｉｃｔｏｒ）は、ＧＰＳ衛星の将来位置を決定する方法である。この方法によって早目に最適星座を選択することが可能になる。
【００１７】
（４０）“第２位置推定”（Ｓｅｃｏｎｄ  ｐｏｓｉｔｉｏｎ  ｅｓｔｉｍａｔｅ）または“ＳＰＥ”は、どのような形状であっても、ＭＰＳから出力される何れかのビークルの推定絶対位置のことである。第２位置推定は、少なくともＩＲＵからの位置情報を含む。第２位置推定は、ビークル上に位置しているビークル走行距離計からの位置情報を含むことができる。
（４１）“空間バイアス”は、二次元または三次元空間における位置の近似に関連するバイアスのことである。空間バイアスは、位置推定の精度を高めるために位置推定をオフセットさせるのに使用される。空間バイアスは、本発明の複数の異なる方法で計算することができる。これらの方法に含まれるのは、例えば、原形バイアス技術１５００（ＩＩ．Ｆ．２．ａ．）、放物線バイアス技術１６００（ＩＩ．Ｆ．２．ｂ．）、べ一ス残留バイアス技術１７００（ＩＩ．Ｆ．２．ｃ．）、及びべ一ス相関器バイアス技術１７００Ａ（ＩＩ．Ｆ．２．ｄ．）である。
（４２）“システム”（Ｓｙｓｔｅｍ）は、装置、方法、または装置と方法の両者の組合せを意味するために簡略化の目的で使用される。更に、これはソフトウェア、ハードウエア、またはハードウエアとソフトウエアの両者の組合せをも含むことができる。
（４３）“位置決定システム”（Ｐｏｓｉｔｉｏｎ  ｄｅｔｅｒｍｉｎａｔｉｏｎ  ｓｙｓｔｅｍ）は、源と受信機との間の相対距離を推定するために信号の受信機が使用することができる信号を発する源を有する何等かのシステムを意味する。信号は、例えば、電磁波、衝撃波、及び音響波の全てまたは何れかの形状であることができる。
（４４）“地上位置決定システム”（Ｔｅｒｒｅｓｔｒｉａｌ  Ｐｏｓｉｔｉｏｎ  ｄｅｔｅｒｍｉｎａｔｉｏｎ  ｓｙｓｔｅｍ）は、地球受信機の地上位置を最終的に推定するために使用できる何等かの位置決定システムを意味する。信号は、例えば、電磁波、衝撃波、及び音響波の全てまたは何れかの形状であることができる。好ましい実施例では、地上位置決定システムはＮＡＶＳＴＡＲ  ＧＰＳである。
（４５）“第３位置推定”（Ｔｈｉｒｄ  ｐｏｓｉｔｉｏｎ  ｅｓｔｉｍａｔｅ）または“ＴＰＥ”は、どのような形状においても、ＶＰＳから出力される何れかのビークルの推定絶対位置のことである。第３位置推定は、第１及び第２位置推定よりも正確なビークル位置の位置推定である、第３位置は、ＶＰＳ処理システムによって第１及び第２位置推定から導出される。
（４６）”ビークル”（Ｖｅｈｉｃ１ｅ）とは、物理的な物を輸送するための何等かの運搬装置のことである。ビークルは、採鉱用トラック、建設用トラック、農業用トラクタ、自動車、船、ボート、列車、気球、ミサイル、または航空機の形状をとることができる。好ましい実施例では、キャタピラ社の７８５オフハイウエイトラックが使用されている。
（４７）”ビークル位置特定システム”（Ｖｅｈｉｃｌｅ  Ｐｏｓｉｔｉｏｎｉｎｇ  ｓｙｓｔｅｍ）または”ＶＰＳ”は、何れかのビークルの位置推定を導出するための本発明のシステムである。ＶＰＳからの位置推定は極めて正確であり、ビークルを正確に案内するために如何なるビークル上の航法システムも使用することができる。好ましい実施例では、ＶＰＳからの位置推定を第３位置推定と呼ぶ。
（４８）“ＶＰＳ処理システム”（ＶＰＳ  Ｐｒｏｃｅｓｓｉｎｇ  ｓｙｓｔｅｍ）は、ＶＰＳの処理システムを意味する。ＶＰＳ処理システムは、第１及び第２位置推定から第３位置推定を導出する。アーキテクチャは図１０及び１１に示されている。
（４９）“重み付きコンバイナ”（Ｗｅｉｇｈｔｅｄ  ｃｏｍｂｉｎｅｒ）は、データを処理する特定のソフトウエアプログラムのことである。入力されたデータは、そのデータの推定精度と、そのデータを集めるために使用された技術とに基づいて所定の重み付け係数が割り当てられる。例えば、好ましい実施例では、ＧＰＳ信号７１６の第１位置推定は、ＩＲＵ信号９１０の第２位置推定よりも重い重み付けがなされる（前者の方が本質的により正確であるから）。更に、ＩＲＵによって測定された速度はＧＰＳ処理システムによって測定された速度よりも重い重み付けがなされる（前者の方が本質的により正確であるから）。好ましい実施例では、ＧＰＳ処理システムによって測定された速度は最後まで使用されないが、他の実施において使用することは可能である。
（５０）“重み付き経路履歴技術”（Ｗｅｉｇｈｔｅｄ  ｐａｔｈ  ｈｉｓｔｏｒｙ  ｔｅｃｈｎｉｑｕｅ）はＧＰＳ処理システムから出力される第１位置推定の精度を高めるための方法または処理過程である。この技術は、将来の第１位置推定の有効性を試験するためのビークル経路モデルを導出するために、それまでの第１位置推定を使用する。重み付き経路履歴技術を使用することにより、第１位置推定の漂動が減少し、偽位置を計算しない免除特性が高められる。
【００１８】
ＩＩ．一般的な概要
図１は、本発明の好ましい実施例の高水準ブロック線図１００を示す。地表の、または地表付近のビークル１０２の正確な自律動作を得るために、本発明はビークル位置特定システム（ＶＰＳ）１０００と、航法システム１０２２とを含む。これらの両システムは装置、方法、及び技術を含み、これらは一緒に統合すると人手によらないビークルの高度に正確な制御を提供する。
Ａ．ビークル位置特定システム（ＶＰＳ）
規定された経路に沿って自律ビークル１０２を案内するタスクは、特に、ある参照点に対するビークルの現位置の正確な推定を必要とする。一旦現位置が知れれば、ビークル１０２はその次の行先へ進むように命令される。
本発明のＶＰＳ１０００を使用すると、ビークル１０２の位置推定を極めて精密に決定することができる。ＶＰＳ１０００は、ＮＡＶＳＴＡＲ  ＧＰＳまたはＣＬＯＮＡＳＳ  ＧＰＳのようなＧＰＳのＧＰＳ衛星１０４からＧＰＳデータを受信する。
好ましい実施例では、ＮＡＶＳＴＡＲ  ＧＰＳを使用している。図１ＡにＮＡＶＳＴＡＲ  ＧＰＳを示す。ＧＰＳ衛星１３０−１６８は６つの軌道１７４−１８４内で地球１７２を周回している。
図１に戻って、ＶＰＳ１Ｏ００は、１または複数の擬似衛星１０５からの擬似衛星データをも受信できる。本明細書においては、“擬似衛星”とは地表の、または地表付近のＧＰＳ衛星に似せた放射装置を意味する。
ＧＰＳデータ及び擬似衛星データの両方または何れか一方から、ＶＰＳ１０００はビークル１０２の正確な位置の推定を導出する。ＧＰＳデータ及び擬似衛星データの両方または何れか一方は、ビークル位置推定の精度を向上させるための本発明の種々の創意に富む技術及び方法によって、大幅に改善される。
詳述すれば、好ましい実施例のＶＰＳ１０００は、ＮＡＶＳＴＡＲＧＰＳ１０４からの、及び移動体位置特定システム９００からのＧＰＳデータを組入れた位置特定システムである。好ましい実施例では、移動体位置特定システム９００は慣性参照ユニット（ＩＲＵ）９０４及びビークル走行距離計９０２の両方または何れか一方を備えている。ＩＲＵ９０４は、１またはそれ以上のレーザジャイロスコープ１０６と、１またはそれ以上の加速度計１０８とを含み、これらは位置、速度、横揺れ、縦揺れ及び偏揺れデータを発生させるために使用できる。ビークル走行距離計９０２は、ビークル１０２が走行した距離に関するデータを発生する。
【００１９】
ビークル１０２の第１位置推定は、ＧＰＳ衛星１４０から受信したＧＰＳデータ及び１または複数の擬似衛星１０５から受信した擬似衛星データから、ＧＰＳ処理システム７００によって導出される。第１位置推定の精度を高めるために、本発明は以下に説明する複数の方法を実現する。更に、ＩＲＵ９０４及びビークル走行距離計９０２の両方または何れか一方を備えた移動体位置特定システム９００のＭＰＳ相互通信プロセッサ９０６によって第２位置推定を導出する。
矢印１１２及び１１４によってそれぞれ示しであるように、第１位置推定及び第２位置推定は、ＶＰＳ処理システム１１６によって組合わされ、濾波される。出力矢印１１８によって示される結果が、より正確な第３位置推定である。
【００２０】
Ｂ．航法システム
航法システム１０２２は、ＶＰＳ１ＯＯＯから第３位置推定を受信する。航法システム１０２２はこの精密な第３位置推定を使用してビークル１０２を正確に航行させる。航法システム１０２２の主目的は、事前に確立された、または動的に生成される経路に沿う点間でビークル１０２を案内することである。
好ましい実施例では、航法システム１０２２はビークル１０２自体上に位置決めされている。換言すれば、それは本質的に“搭載”システムである。更に・航法システム１０２２は、既存ビークル１０２に事後設置するように設計することができる。
航法システム１０２２がビークル１０２を案内して事前に確立された、または動的に生成される経路を辿らせることを可能にするために、種々のモデル、即ち概念的表現が生成され、使用される。例えば、線及び弧を使用して目標点間にビークル経路を確立することができる。ビークル１０２が航行する実際の経路をモデル化するために、数学的Ｂスプラインまたはクロソイド（ｃｌｏｔｈｏｉｄ）曲線を使用することができる。これらの数学的曲線に関しては後に詳述する。
上記モデル化、即ち表現技術の使用が、データ通信、記憶、及びビークル１０２の取り扱いを向上させる。更に、これらの技術が制御及び通信の階層を提供するので、監視タスクを簡易化することができる。階層制御体系に存在する制御のレベルが高いほど、タスクは簡易化され、また命令はコンパクトになる。
航法システム１０２２は、ビークル１０２を移動させ、停止させ、舵取りするために要求される必要物理動作を遂行するために、ブレーキ、舵取り装置、及び機関及び伝動装置のようなビークルの機械的システムの制御をも行う。
また航法システム１０２２は、所望の位置に対するビークル１０２の実際の位置を検査し、所望の位置に従ってビークル制御を修正する。航法システム１０２２は、この検査能力を向上させるために、多重状態モデルを走らせることができる。更に航法システム１０２２は、システム自体及びビークル成分内の誤りまたは障害を検査する。もし誤りまたは障害が検出されれば、航法システム１０２２はビークル１０２を完全に停止せしめることによってフェールセーフ運転停止を遂行することができる。
更に航法システム１０２２は、ビークル１０２を制御するための異なるモードを提供する。これらには、（１）ビークル１０２の航行が航法システム１０２２によって自動的に処理される完全自律モードと、（２）遠隔の操作員（図示してない）がビークル１０２の方向及び運動等を制御できるテレモード、または遠隔制御モードと、（３）ビークル１０２内に座している操作員がビークル１０２の制御を行い、それを手動で駆動することができる手動モードとが含まれる。
自律モードでは、もしビークル１０２が制御下になければ、障害物が特性に甚大な損傷をもたらし、生命に重大な危害を与えかねないので、障害物の検出は臨界的である。航法システム１０２２は効率的に障害物を検出することができる。巨礫、動物、人々、樹木、または他の障害物が突然ビークル１０２の経路内に侵入するかも知れない。航法システム１０２はこれらの障害物を検出することができ、ビークル１０２を停止させるか、または障害物を廻る経路を計画し、その経路が安全であると考えられるとビークル１０２を元のルートに戻す。
所望のルートを正確に追尾することが航法システム１０２２の別の機能である。航法システム１０２２の機能及びアーキテクチャは、約３０マイル／時（ｍｐｈ）までの速度でビークル経路を実時間追尾するように設計されている。
【００２１】
Ｃ．べ一スステーション
本発明は、べ一スステーション１８８にホスト処理システム１８６を備えることができる。ホスト処理システム１８６は、ＶＰＳ１０００及び航法システム１０２２の両方に関する機能を遂行する。
ＶＰＳ１０００に関してホスト処理システム１８６は、矢印１９０及び１９２によってそれぞれ示してあるように、ＧＰＳデータ及び擬似衛星データの両方または何れか一方を受信する。奉際には、後述するようにビークル位置推定の精度を改害するために、べ一スステーション１８８と同様にホスト処理システム１８６は既知の参照点として働く。
ホスト処理システム１８６はビークル位置推定の精度を向上させるための種々の方法を実現する。前述の衛星位置予測法（ＩＩ．Ｇ．参照）もホスト処理システム１８６によって実現される。ホスト処理システム１８６は、ビークル１０２によって観測される同一衛星星座を認識するであろう。
ＧＰＳデータ及び擬似衛星データの両方または何れか一方に対して計算が遂行されてバイアスが導出される。本明細書においては“バイアス”とは、２つの測定の間の差のことであり、通常は位置推定間の差（空間バイアス）またはクロックレートの差（時計バイアス）である。通常は一方の測定の方が別の測定よりも正確であることが知られているから、バイアスを“誤差”と呼ぶことが多い。
空間バイアスを計算するために、ホスト処理システム１８６は多くの方法を実現する。これらの方法には、例えば、原形バイアス技術１５００（ＩＩ．Ｆ．２．ａ．）、放物線バイアス技術１６００（ＩＩ．Ｆ．２．ｂ．）、べ一ス残留バイアス技術（ＩＩ．Ｆ．２．ｃ．）、及びべ一ス相関器バイアス技術（１１．Ｆ．２．ｄ．）が含まれる。
【００２２】
前述の差動修正技術はデータ誤差を補償する。換言すれば、ポスト処理システム１８６において計算されたバイアスはデータ誤差を表す。矢印１９４によって示すように、バイアスはビークル１０２のＧＰＳ処理システム７００へ送信される。ＧＰＳ処理システム７００はこれらのバイアスを使用してビークル位置推定内の誤差を排除する。ホスト処理システム１８６は、本発明の航法システム１０２２に関する機能をも提供する。矢印１９６によって示してあるように、ホスト処理システム１８６は、航法システム１０２２の最高水準の制御として働く。それは、人間の運行管理係が達成するのと殆ど同一の結果をもたらすようにビークル１Ｏ２のスケジューリング及び運行管理を処理する。従って、ホスト処理システム１８６は、ビークル１０２の作業サイクルを決定することができる。
ホスト処理システム１８６は、ビークル１０２に指定された経路を通って現位置から将来位置まで進行するように命令するので、ビークル１０２はその作業目的を達成することができる。ホスト処理システム１８６は、普通に行われているようにルートに沿う各点をリストするのではなく、名前によってビークルルートを指定することができる。従って、ビークル搭載航法システム１０２２は名付けられたビークルルートを表引きし、名付けられたビークルルートを名付けられたビークルルートに沿うノード及びセグメントの集合に翻訳する。
【００２３】
ＩＩ．ビークル位置特定システム
Ａ．概要
以下に特に図７乃至２１を参照してＶＰＳ１０００に関して説明する。図１０及び１１はＶＰＳ１０００のアーキテクチャ／ハードウエアを示す。ＶＰＳ１０００は地表の、または地表付近の移動または静止ビークル１０２の極めて正確な位置決定システムである。
ＶＰＳ１０００が、それぞれ図７及び９に示すＧＰＳ処理システム７００及びＭＰＳ９００を含んでいることを思い出されたい。またＭＰＳ９００が、共に図９に示されているＩＲＵ９０４とビークル走行距離計９０２とを含んでいることも思い出されたい。実際には、極めて効果的な位置決定システムを得るために、これらのシステムは本発明によって向上せしめられている。
図７を参照する。ＧＰＳ処理システム７００は、ＧＰＳ受信機７０６に接続されるアンテナ７０２を含んでいる。アンテナ７０２の視野内のＧＰＳ衛星１０４が、図２及び３に示すように複数のＧＰＳ衛星２００−２０６からなっている場合には、ＧＰＳ受信機７０６はそれらの各ＧＰＳデータを、アンテナ７０２の視野内の１またはそれ以上の何れかの擬似衛星１０５からの擬似衛星データと共に読み取る。好ましい実施例では、ＧＰＳ受信機７０６はビークル１０２の第１位置推定をＧＰＳデータ及び擬似衛星データの両方または何れか一方から計算する責を負っている。
第１位置方法の精度を高めるために、衛星位置予測方法１８００（ＩＩ．Ｇ．参照）がＧＰＳ処理システム７００のＧＰＳプロセッサ７１０によって実現される。衛星位置予測方法１８００は現時点における、または何れかの将来時点における如何なるＧＰＳ衛星の位置をも予測する。
【００２４】
衛星位置情報を使用すれば、ＧＰＳ処理システム７００は最適ＧＰＳ衛星星座を決定でき、星座効果方法１３００（ＩＩ．Ｆ．参照）を使用することによって認識することができる。好ましい実施例では、星座効果方法１３００もＧＰＳプロセッサ７１０によって実現されている。星座効果方法１３００に従えば、ＧＰＳ衛星２００−２０６及び１または複数の擬似衛星１０５からなるデータ源から最良の星座が選択される。
ＧＰＳプロセッサ７０６は、最良星座及び幾何学／三角法に基づいてビークル１０２の第１位置推定を計算する。第１位置推定の精度は、部分的に、計算に用いられるＧＰＳ衛星の数に依存する。使用される付加的な各ＧＰＳ衛星は第１位置推定の精度を向上させることができる、計算の後に、ビークル１０２の第１位置推定は図１０のＶＰＳ主プロセッサ１００２へ送信される。
図９を参照する、ＩＲＵ９０４は、位置、速度、横揺れ、縦揺れ、偏揺れデータを発生するレーザジャイロスコープ及び加速度計を備えている。ＩＲＵ９０４は、この情報をビークル１０２の第２位置推定内へ組合わせる。ビークル１０２が走行した距離を測定するために走行距離計９０２を組込むことができる。図１０に示すように、ＩＲＵ９０４及び走行距離計９０２からのデータもＭＰＳ相互通信プロセッサ９０６を介してＶＰＳ主プロセッサ１００２へ送信される。
ＶＰＳ主プロセッサ１００２は、ＭＰＳ９００（ＩＲＵ９０４及び走行距離計９０２の場合もある）からの第２位置推定と、ＧＰＳ処理システム７００からの第１位置推定とを組合わせて、より正確な第３位置推定を生成する。
ＶＰＳ１０００は、ビークルを“漂動”させる恐れのある誤りの、または偽の第３位置推定を排除する方法をも実現する。この方法は重み付き経路履歴方法（ＩＩ．Ｈ．参照）と呼ばれる。本質的に、ビークル１０２の経路履歴は、ビークル１０２の位置の将来推定の精度を統計的に決定するために使用される。
【００２５】
図１及び３を参照する。べ一スステーション１８８は、ＶＰＳ１Ｏ００のための地理的近似参照点を提供する、べ一スステーション１８８は、ホスト処理システム１８６を含む。好ましい実施例では、ホスト処理システム１８６はＧＰＳ処理システム７００と類似のアーキテクチャを備え、同一機能を遂行する。しかし、ホスト処理システム７００は、第１位置推定の精度を向上させるための付加的な機能を遂行する。
衛星位置予測方法１８００（ＩＩ．Ｇ．）は、上述のようにＧＰＳ処理システム７００に加えてホスト処理システム１８６によって実現される。従ってホスト処理システム１８６は、ビークル１０２が観測したものと同一の、またはより大きい星座内に同一ＧＰＳ衛星を含むＧＰＳ衛星星座を認識する。
ＧＰＳデータ及び擬似衛星データの両方または何れか一方に対して計算が遂行され、空間バイアス及び時計バイアスを含むバイアスが導出される。空間バイアスを計算するために、ホスト処理システム１８６は種々の方法を実現する。図１５は原形バイアス技術１５００（ＩＩ．Ｆ．２．ａ．）を示す。図１６は放物線バイアス技術１６００（ＩＩ．Ｆ．２．ｂ．）を示す。図１７はべ一ス残留バイアス技術１７００（ＩＩ．Ｆ．２．ｃ．）を示す。図１７Ａはべ一ス相関器バイアス技術１７００Ａ（１１．Ｆ．２．ｄ．）を示す。
矢印１９４で示すように、空間バイアス及び時計バイアスはビークル１０２のＧＰＳ処理システム７００へ送信される。ＧＰＳ処理システム７００はこれらのバイアスを使用してビークル位置推定内の誤差を排除する。
【００２６】
Ｂ．  ＧＰＳ処理システム
ＧＰＳ処理システム７００は、地上位置決定システムからのビークル位置データを使用してビークル１０２の第１位置推定を導出する。好ましい実施例では、地上位置決定システムは、現在Ｕ．Ｓ．政府が開発中のＮＡＶＳＴＡＲ  ＧＰＳ及び地球をべ一スとする擬似衛星の両方または何れか一方からなる。
１．  ＮＡＶＳＴＡＲ  ＧＰＳ
図１Ａに示すように、ＮＡＶＳＴＡＲ  ＧＰＳは、現在では６つの軌道１７４−１８４内に２４個の人工電子ＧＰＳ衛星１３２−１７０を企図している。現在考えられているところでは、ＧＰＳ衛星１３２−１７０は約１４，Ｏ００マイルの高度で、１日に２回地球１７２を周回する。後述するように、ＮＡＶＳＴＡＲ  ＧＰＳのＣモードを使用すると天候、時刻には関係なく、また地球１７２の殆どの領域において１５メートル以内で地上位置を決定することが可能である。
本明細書を提出した日現在で、地球１７２を回る軌道内には６個の実験用ＧＰＳ衛星と、７個の動作中のＧＰＳ衛星とが存在することが知られている。更に、幾つかの製造業者が、図７のＧＰＳ受信機７０６のようなＧＰＳ受信機を設計または、製造中であることが知られている。ＧＰＳ衛星は次々に開発されて機能しており、位置追尾のために毎日使用できる実験用ＧＰＳ衛星が３個またはそれ以上である時間は増加している。
更に、実験用ＧＰＳ衛星は（及び他の全ては、一旦展開すると）正確に予測可能である。ビークル１０２上のＧＰＳ受信機に対するこれらのＧＰＳ衛星の相対位置、即ち“擬似距離”は、電磁信号から２つの方法によって決定できる。
１つの方法は、発射される電磁信号の送信と受信との間の伝播時間遅延を測定することである。ＮＡＶＳＴＡＲ  ＧＰＳでは、電磁信号は、それらの信号がＧＰＳ衛星から送信される時刻で連続的に符号化されている。受信時刻に注目して、符号化された送信時刻を差し引くことによって時間遅延を導出できることは言うまでもない。計算された時間遅延と、大気中の電磁波の走行速度とから、擬似距離を正確に導出することができる。上記方法を使用して計算された擬似距離を、本明細書では“実際の”擬似距離と呼ぶ。
別の方法は、周回するＧＰＳ衛星から送信される電磁信号内に符号化されている衛星位置データを包含する。ＮＡＶＳＴＡＲ  ＧＰＳのＧＰＳ衛星の位置データに関する暦データが公的に入手できる。もし受信機位置が知られていれば、電磁信号内に符号化されているデータに関するこの暦データを参照することによって、擬似距離の正確な導出が可能である。この方法を使用して計算された擬似距離を、本明細書では“推定”擬似距離と呼ぶ。
しかしながら、推定擬似距離を導出する従来の方法においては、衛星位置データがＧＰＳ衛星において毎正時に１回だけしか更新されないことに注目すべきである。従って、更新された衛星位置データを使用して新しい擬似距離を計算する場合には、擬似距離の精度は次の正時までは各正時からの時間と共に低下する。
再び図１Ａを参照する。この図には完全に機能しているＮＡＶＳＴＡＲ  ＧＰＳの構成の概要が示されている。２４個の各ＧＰＳ衛星１３２−１７０は電磁信号を送信し、これらの電磁信号はビークル１０２の絶対地上位置（即ち、地球１７２の中心に対するする緯度、経度、及び高度）を決定るために使用することができる。
【００２７】
具体的に言えば、少なくとも３個の周回ＧＰＳ衛星１３２−１７０の相対位置を知ることによって、三角法を含む簡単な幾何学理論を使用してビークル１０２の絶対地上位置を計算することができる。この地上位置推定の精度は、部分的に、ビークル１０２がサンプルする周回ＧＰＳ衛星１３２−１７０の数に依存する。より多くのＧＰＳ衛星１３２−１７０のサンプリングを計算に入れれば、地上位置推定の精度は向上する。ビークル１０２の回路と種々のＧＰＳ衛星１３２−１７０との間の回路時計差が誤差をもたらすので、伝統的に、３個ではなく４個のＧＰＳ衛星をサンプルして各地上位置推定を決定している。
ＮＡＶＳＴＡＲ  ＧＰＳでは、電磁信号は全てのＧＰＳ衛星１３２−１７０から単一の搬送波周波数で連続的に送信される。しかし、各ＧＰＳ衛星１３２−１７０は異なる変調計画を有し、それによって電磁信号を区別できるようになっている。ＮＡＶＳＴＡＲ  ＧＰＳでは、搬送波周波数は、各ＧＰＳ衛星に独特な擬似２進コード信号（データビット流）を使用して変調されている。この擬似２進コード信号が搬送波周波数を２相変調するために使用される。従って搬送波周波数を復調すればＮＡＶＳＴＡＲ  ＧＰＳ内の周回ＧＰＳ衛星を識別することができる。
【００２８】
更にＮＡＶＳＴＡＲ  ＧＰＳには、擬似ランダム数（ＰＲＮ）信号を使用する２つの搬送波変調モードが含まれる。“粗／収集”（Ｃ／Ａ）モードと呼ぶ一方のモードでは、ＰＲＮ信号は１．０２３ＭＨｚのチップレートを有するゴールドコードシーケンスである。ゴールドコードシーケンスは当分野では周知の、普通の擬似ランダムシーケンスである。擬似ランダムコードシーケンスのチップレートは、シーケンス内にチップが生成されるレートである。従って、チップレートは、コード繰り返しレートをそのコード内のメンバーの数で除した値に等しい。従って、ＮＡＶＳＴＡＲ  ＧＰＳの粗／収集モードでは、各ゴールドコードシーケンス内に１，０２３チップが存在し、シーケンスは１ミリ秒毎に繰り返される。４個の周回ＧＰＳ衛星からの１．０２３ＭＨｚのゴールドコードシーケンスを使用すると、ビークル１０２の地上位置を６０乃至３００メートル以内の近似精度で決定することができる。
ＮＡＶＳＴＡＲ  ＧＰＳの第２の変調モードは、一般に“精密”または“保護”（Ｐ）モ一ドと呼ばれている。Ｐモードでは、擬似ランダムコードは１０．２３ＭＨｚのチップレートを有している。更に、Ｐモードシーケンスは極めて長く、シーケンスは２６７日に１回以上繰り返すことはない。その結果、ビークル１０２の地上位置を１６乃至３０メートルの近似精度で決定することができる。
しかしながら、Ｐモードシーケィスは機密扱いされており、合衆国政府によって一般には利用できないようになっている。換言すれば、Ｐモードは合衆国政府によって承認された地球受信機だけが使用するようになっているのである。
地球受信機が、異なる周回ＧＰＳ衛星からの種々のＣ／Ａ信号を区別できるようにするために、通常は地球受信機は局部的に生成されるゴールドコードシーケンスのための複数の異なるゴールドコード源を含んでいる。局部的に導出される各ゴ一ルドコードシーケンスは、各ＧＰＳ衛星からの各独特なゴールドコードシーケンスに対応する。
局部的に導出されたゴールドコードシーケンスと、送信されたゴールドコードシーケンスとは、１ミリ秒のゴールドコードシーケンス周期にわたって互いに相互相関される。局部的に導出されるゴールドコードシーケンスの位相は、最大の相互相関関数が得られるまでチップ毎に、次いでチップ内で変化する。１，０２３ビットの長さを有する２つのゴールドコードシーケンスの相互相関は、他の如何なるゴールドコードシーケンスの組合わせの相互相関関数よりほぼ１６倍は大きいから、局部的に導出されたゴールドコードシーケンスをＧＰＳ衛星の１つから送信された同一ゴールドコードシーケンスにロックすることは比較的容易である。
【００２９】
地球受信機の視野内の少なくとも４個のＧＰＳ衛星からのゴールドコードシーケンスは、このようにして、局部的に導出された各ゴールドコードシーケンスに順次に応答する単一のチャネルを使用するか、または代替として、異なるゴールドコードシーケンスに同時に応答する並列チャネルを使用することによって分離される。４つのゴールドコードシーケンスを地球受信機の視野内の４個のＧＰＳ衛星から受信したゴールドコードシーケンスと同相にロックした後は、地球受信機の相対位置は約６０乃至３００メートルの精度で決定できる。
ＮＡＶＳＴＡＲ  ＧＰＳの上記近似精度は、（１）地球受信機が実際に応答する信号を送信中のＧＰＳ衛星の数と、（２）受信信号の可変振幅と、（３）異なるＧＰＳ衛星から受信した信号間の相互相関ピークの大きさとによって影響を受ける。
【００３０】
図７を参照する。ＧＰＳ処理システム７００はＧＰＳ衛星１３２−１７０からのＧＰＳデータ及び１または複数の擬似衛星１０５からの擬似衛星データを処理する。更に、ＧＰＳ受信機７０６は種々のＧＰＳ衛星１３２−１７０からのＣ／Ａ信号を解号する。
図２に、ＮＡＶＳＴＡＲ  ＧＰＳの４個のＧＰＳ衛星２００−２０６に関する航法方程式２１２を示す。４個のＧＰＳ衛星２００、２０２、２０４及び２０６はそれぞれ擬似距離Ｒ０、Ｒ２、Ｒ４、及びＲ６を有し、ビークル１０２によって認識されたＧＰＳ衛星１３２−１７０の現星座を構成している。航法方程式２１２は、ＧＰＳ衛星２００−２０６とビークル１０２との間の時計バイアスＣｂを含む。航法方程式２１２は擬似距離Ｒ０、Ｒ２、Ｒ４、及びＲ６を使用してビークル１０２の経度及び緯度を計算するために使用される。
記述ブロック２０８内に示すように、各ＧＰＳ衛星２００、２０２、２０４及び２０６は、タイミングデータ（ＧＰＳ時刻）と天体暦データとを含むＧＰＳデータを送信する。当分野では周知の航法方程式２１２と、上記タイミングデータとを使用すれば、ＧＰＳ処理システム７００は擬似距離ＲＯ、Ｒ２、Ｒ４、及びＲ６（実際の擬似距離と呼ぶ）を推定することができる。更に、上記天体暦データと地球１７２上の暦データを使用すれば、ＧＰＳ処理システムは擬似距離ＲＯ、Ｒ２、Ｒ４、及びＲ６（推定擬似距離と呼ぶ）を推定することができる。
【００３１】
２．動作
図６に、動作中の代表的なＧＰＳ星座を示す。４個のＧＰＳ衛星２００、２０２、２０４及び２０６はＧＰＳデータを送信する。ビークル１０２及びべ一スステーション１８８の両者は、これらの各ＧＰＳ衛星２００、２０２、２０４及び２０６からのこれらの信号を、それぞれＧＰＳアンテナ３１２及び３１６で受信する。好ましい実施例では、Ｃ／Ａコード及び搬送波周波数の両者がＧＰＳアンテナ３１２及び３１６で受信され、処理される。
図６には、４個のＧＰＳ衛星の他に、擬似衛星１０５が示されている。図６に示すように、１または複数の擬似衛星１０５はどのような鉱坑の境界の周囲に戦略的に配置することもでき、またＧＰＳ衛星２００、２０２、２０４及び２０６に似せることができる。この配列は、高い鉱坑壁のような地形的な特色のために採鉱ビークルがＧＰＳ衛星２００、２０２、２０４及び２０６の１またはそれ以上の視界から消えるような鉱坑、空洞等のような状況に極めて有用とすることができる。大地をべ一スとする１または複数の擬似衛星１０５は付加的な測距信号を供給し、従って本発明における位置決め能力の可用性（利用可能度）及び精度を改善することができる。
１または複数の擬似衛星１０５はＧＰＳ衛星２００、２０２、２０４及び２０６と同期されており、ＧＰＳ衛星２００、２０２、２０４及び２０６の信号構造とは異なってはいるが両立可能な信号構造を有している。更に、ビークル１０２と１または複数の擬似衛星１０５との間の距離（レンジ）は、ビークル１０２とＧＰＳ衛星２００、２０２、２０４及び２０６の１つとの間の距離と同様に計算される。１または複数の擬似衛星１０５を使用すれば、測距誤差は選択可用性も、電離層誤差も含まない。しかし対流圏誤差、擬似衛星時計誤差、及び多重経路誤差のような他の誤差は斟酌しなければならない。
深い露天堀り採鉱作業では、鉱坑内のビークル１０２からの空の視界は、鉱坑壁によって制限されることがあり得る。そのためＧＰＳ処理システム７００の視界内には、第１位置推定を適切に導出するのに十分な数のＧＰＳ衛星は存在しないかも知れない。このような場合、本発明では、１またはそれ以上の擬似衛星１０５を二次的な源として働かせることができる。１または複数の擬似衛星１０５を鉱山の縁に、または他の場所に配置することができる。１または複数の擬似衛星１０５は、正確な第１位置推定を得るために何れかの可視ＧＰＳ衛星と共に使用することができる。
ＧＰＳ衛星を支援するために、またはＧＰＳ衛星からＧＰＳデータを受信する必要を完全に排除するような他の形状の二次的な源も実現可能である。更に、レーザ走査技術を利用して二次的な参照源からビークル１０２までの局所化された測距データを得ることができる。
【００３２】
通信チャネル６１８は、べ一スステーション１８８とビークル１０２との間の通信リンクを表す。好ましい実施例では、通信チャネル６１８は送受信機であるデータ無線機６２０及び６２２によって確立された電磁リンクからなっている。通信チャネル６１８は、べ一スステーション１８８とビークル１０２との間のデータ転送に使用される。他の形状の通信媒体も使用できる。例えば、レーザ走査技術を利用してべ一スステーション１８８からビークル１０２へ情報を輸送することができる。
データ無線機６２０及び６２２は、それぞれべ一スステーション１８８及びビークル１０２に配置されている。無線機６２０及び６２２は、べ一スステーション１８８とビークル１０２との間のデータ交換の責を負う、データ交換の型に関しては後述する。
好ましい実施例においてデータ無線機６２０及び６２２として適切に機能する無線送受信機は、カナダ国モントリオールのＤａｔａｒａｄｉｏ  Ｌｔｄ．からモデルＤＲ−４８００ＢＺとして市販されている。
【００３３】
図７に、ＧＰＳ処理システム７００の好ましい実施例を示す。ビークル１０２上のＧＰＳ処理システム７００はＧＰＳアンテナ７０２を含む。好ましい実施例では、ＧＰＳアンテナ７０２は電磁放射の無線スペクトルを受け入れる。しかし、本発明はＧＰＳ衛星１３２−１７０がデータを符号化することができる如何なる信号をも受信することを企図するものである。好ましい実施例では、ＧＰＳアンテナ７０２は、マサチュセッツ州リトルトンのＣｈｕ  Ａｓｓｏｃｉａｔｅｓ  Ｉｎｃ。からモデルＣＡ３２２４として市販されているアンテナである。
ＧＰＳアンテナ７０２は前置増幅器７０４に結合されているので、ＧＰＳアンテナ７０２が受信した信号は前置増幅器７０４へ伝送することができる。本明細書では”結合”とは、通信を確立するための何等かのシステム及び方法を意味している。結合システム及び方法は、例えば電子、光学、音響その他の技術の全てまたは何れかを含むが、詳細な説明は省略する。好ましい実施例では、結合は一般に電子式であり、数多くの工業標準の電子インタフェースの何れか１つを考えている。
前置増幅器７０４はＧＰＳアンテナ７０２から受信したＧＰＳデータを増幅し、それを処理し解号できるように変換する。本発明は、受信信号を増幅できる何等かの方法を企図している、好ましい実施例では、前置増幅器７０４は、カリフォルニア州サンタクララのＴｅｌｅｃｏｍｍｕｉｃａｔｉｏｎｓ  Ｉｎｃ．（ＳＴｅｌ）から市販されているモデル５３００シリーズＧＰＳ  ＲＦ／ＩＦである。前置増幅器７０４は、ＧＰＳ受信機７０６に結合されている。ＧＰＳ受信機７０６は、ＧＰＳアンテナ７０２の視界内のＧＰＳ衛星２００、２０２、２０４及び２０６から送られたＧＰＳデータを処理する。各ＧＰＳ受信機７０６は、ＧＰＳ衛星２００、２０２、２０４及び２０６毎の実際の擬似距離を計算する。本明細書では実際の擬似距離は、ＧＰＳ衛星からの電磁信号の送信と、ＧＰＳ処理システム７００による電磁信号の受信との間の時間遅延から導出された擬似距離の推定ＲＯ、Ｒ２、Ｒ４及びＲ６として定義されている。更に好ましい実施例ではＧＰＳ受信機７０６は、ＧＰＳ衛星２００、２０２、２０４及び２０６に関する実際の擬似距離の全てを並列に処理できる。
【００３４】
本発明の好ましい実施例では、ＧＰＳ受信機７０６は、４個またはそれ以上のＧＰＳ衛星が見えている時にこのデータを生成する。本明細書のＩＩ．Ｆ．２．において説明する差動修正技術を使用すれば、ＧＰＳ処理システム７００は、４個のＧＰＳ衛星２００、２０２、２０４及び２０６の最適星座が見える時には約２５メートルの精度で第１位置推定を（ＧＰＳプロセッサ７１０において）計算することができる。視界内に５個のＧＰＳ衛星の最適星座がある時には（図示してない）、約１５メートルの精度で第１位置推定を計算することができる。“最適”星座とは、空間内のＧＰＳ衛星の相対位置が周知の優れた三角測量能力を提供するような星座のことである。
好ましい実施例では、ＧＰＳ受信機７０６は実際の擬似距離と、現在サンプル中のＧＰＳ衛星１３２−１７０の数とを出力する。一連の第１位置推定のために見えているＧＰＳ衛星１３２−１７０の数が４個より少ない場合には、好ましい実施例のＶＰＳ重み付きコンバイナ１２０４（図１２及びその説明参照）は、ＧＰＳ処理システム７００（具体的にはＧＰＳプロセッサ７１０）から受信した第１位置推定を第３位置推定の計算には使用しない。
【００３５】
好ましい実施例のＧＰＳ受信機７０６は、Ｓｔａｎｆｏｒｄ  Ｔｅ１ｅｃｏｍｍｕｎｉｃａｔｉｏｎｓ  Ｉｎｃ．製モデル５３０５−ＮＳＩ受信機からなる。しかし、実際の擬似距離と、サンブル中のＧＰＳ衛星の数とを出力できる如何なる受信機を使用しても差し支えない。
好ましい実施例に使用されている受信機の型のために、ＧＰＳ受信機７０６はＧＰＳ相互通信プロセッサ７０８に結合されている。好ましい実施例では、相互通信プロセッサ７０８はＵ．Ｓ．Ａ．イリノイ州ショームバーグのＭｏｔｏｒｏｌａ  Ｉｎｃ．から市販されている６８０００マイクロプロセッサである。以下に説明する目的と同一の目的を遂行するために、如何なるプロセッサも単独で、またはＧＰＳ受信機７０６と組合せて使用することができる。
ＧＰＳ相互通信プロセッサ７０８は更に、ＧＰＳプロセッサ７１０及びＧＰＳコンソール１７１２に結合されている。ＧＰＳ相互通信プロセッサ７０８は、これら３つの装置間のデータ交換を調製する。即ち、ＧＰＳ相互通信プロセッサ７０８は、ＧＰＳ受信機７０６から受信した擬似距離データをＧＰＳプロセッサ７１０へ渡す。擬似距離は、例えばＧＰＳ受信機７０６が計算した実際の擬似距離と、ＧＰＳ受信機７０６が現在見ているＧＰＳ衛星２００、２０２、２０４及び２０６の数と、各ＧＰＳ衛星２００、２０２、２０４及び２０６毎にＧＰＳプロセッサ７１０が推定擬似距離を計算するために必要な他のＧＰＳデータとを含む。ＧＰＳ相互通信プロセッサ７０８は、ＧＰＳ受信機７０６及びＧＰＳプロセッサ７１０に関するステータス情報もＧＰＳコンソール１７１２へ中継する。
ＧＰＳ相互通信プロセッサ７０８は、上記情報をＧＰＳプロセッサ７１０へ伝送する。好ましい実施例では、ＧＰＳプロセッサ７１０はＭｏｔｏｒｏｌａ  Ｉｎｃ．から市販されている６８０２０マイクロプロセッサからなる。図８はＧＰＳプロセッサ７１０内のソフトウエアの機能を示す低水準流れ図８００である。
ＧＰＳプロセッサ７１０は受信したデータを処理するために、例えば図８に示すＧＰＳカルマンフィルタ８０２を含む多くのアルゴリズム及び方法を使用する。カルマンフィルタ８０２は当分野では公知である。好ましい実施例では、ＧＰＳカルマンフィルタ８０２はＧＰＳプロセッサ７１Ｏのソフトウェア内のモジュールである。
【００３６】
カルマンフィルタ８０２の機能は、部分的に、擬似距離データに付随する雑音を濾波して排除することである。この雑音には、例えば電離層、時計及び受信機の全てまたは何れかの雑音が含まれる。べ一スステーション１８８に配置されているホスト処理システム１８６のＧＰＳカルマンフィルタ８０２は、第１位置推定の精度を向上させるために（本明細書のＩＩ．Ｆ．２．参照）ビークル１０２へ伝送される空間バイアス及び時計バイアスを計算する。これに対してビークル１０２内のＧＰＳカルマンフィルタ８０２は、べ一スステーション１８８から受信する空間バイアス及び時計バイアスを斟酌する。
ＧＰＳカルマンフィルタ８０２は、半適応的に機能する。換言すれば、ＧＰＳカルマンフィルタ８０２はビークル１０２の速度に依存して、受け入れ可能なデータ動揺のしきい値を自動的に変更する。本明細書では“動揺”とは、正規のコースからのずれを意味する。ＧＰＳカルマンフィルタ８０２の半適応的な機能が、本発明の応答及び精度を最適化する。
要約すれば、ビークル１０２がその速度を指定された量だけ増加させると、ＧＰＳカルマンフィルタ８０２はその受け入れ可能なデータ動揺のしきい値を上昇させる。同様に、ビークル１０２がその速度を指定された量だけ減少させると、ＧＰＳカルマンフィルタ８０２はその受け入れ可能なデータ動揺のしきい値を低下させる。本発明のこの自動最適化技術は、移動及び静止の両状態の下で最高度の精度を提供する。
【００３７】
本発明の最良モードでは、ＧＰＳカルマンフィルタ８０２のしきい値は連続的に、または極めて微小な離散した間隔で変化するのではない。そうではなく、これらの間隔はより大きく離散した間隔であり、従って、連続的に変化するフィルタより精度は低下する。しかし、本発明のカルマンフィルタ８０２は連続的に変化するフィルタより実現が容易であり、低価格であり、計算時間が少なくて済む。しかしながら、連続的に変化するフィルタを使用することは可能であり、またそのことも本発明に含まれていることを理解すべきである。
動作させるためには、システムの始動時にＧＰＳカルマンフィルタ８０２に初期値を与えなければならない。この初期値とＧＰＳ受信機７０６が収集したＧＰＳデータとから、ＧＰＳカルマンフィルタ８０２は現在の状態（第１位置推定と、北進、東進のためのビークル速度及び高度とを含む）を補外する。ＧＰＳカルマンフィルタ８０２は循環的に動作する、換言すれば、補外された現在の状態が次の繰り返しのための初期値であると見なされるのである。これは新しいＧＰＳデータ（更新）と組合わされ／濾波されて新しい現在の状態が導出される。
【００３８】
ＧＰＳデータを使用する方法は、制御ファイル８２０と呼ばれる演繹的に保管されたファイルに依存する。制御ファイル８２０は、（１）雑音しきい値、（２）応答の速さ、（３）ビークル位置及び速度の初期状態、（４）ＧＰＳカルマンフィルタ８０２のリセットが行われる前のずれの程度、（５）許容される不良測定の数、及び（６）測定に割り当てられた時間の全てまたは何れかを決定する。
次いでＧＰＳプロセッサ７１０は上述の現在の状態と、時計バイアス及び空間バイアスを含む何等かのバイアスとを使用して、推定擬似距離と、第１位置推定と、ビークル速度（ドップラ偏移）とを計算する。しかし、ビークル速度を導出するために搬送波周波数ではなくＣ／ＡコードがＧＰＳ受信機７０６によって使用される場合には、ＧＰＳプロセッサ７１０は計算した速度データを破棄する。ビークル速度を破棄する理由は、Ｃ／Ａコードから導出した場合には十分な精度が得られないことが実験から分かっているからである。
搬送波周波数から導出されるビークル速度（ドップラ偏移）はＣ／Ａコードから導出される速度よりも遥かに正確である。好ましい実施例では、図１０に示すように第１推定位置（及び、もし搬送波周波数から導出されれば、ビークル速度）はＧＰＳ信号７１６上に符号化され、ＶＰＳ主プロセッサ１００２へ送られる。
前述のように、ＧＰＳプロセッサ７１０は搬送波周波数及びＣ／Ａコードの両方を解析する。Ｃ／Ａコードから解号されるデータとは異なり、データはＧＰＳ受信機７０６によって搬送波周波数からほぼ５０Ｈｚで（Ｃ／Ａコードを解号する場合の約２Ｈｚではなく）検索することができる。この増大した速さによって、本発明は少ない誤差で、より精密な位置及び速度を決定することができる。
【００３９】
図８には好ましい実施例におけるＧＰＳプロセッサ７１０の他の機能が示されている。しかし本発明は、ＧＰＳデータを処理して擬似距離を決定できる如何なる方（法をも意図するものである。流れ図ブロック８１６に示すように、コンソール機能はＧＰＳコンソール２の動作を制御する。好ましい実施例のコンソール機能は、ユーザインタフェースをフィルタ内に設けることによってＧＰＳカルマンフィルタ８０２の動作を統制する。
ＶＰＳ通信機能８１８はＶＰＳ１０００へ導かれるＧＰＳカルマンフィルタ８０２の出力を制御する。流れ図ブロック８０６に示すように、ＧＰＳカルマンフィルタ８０２はＧＰＳ受信機７０６からのデータを要求し、解号する。このデータは流れ図ブロック８０４に示すようにＩＰＲＯＴ０機能８０４を通して導かれる。図示のように、１ＰＲＯＴ０機能８０４はＧＰＳ相互通信プロセッサ７０８内に常駐し、ＧＰＳ相互通信プロセッサ７０８に関連するタスクを実行する。好ましい実施例では、ＩＰＲＯＴ０機能８０４はＸｙｃｏｍ  Ｉｎｃ．から市販されているモデルＸＶＭＥ−０８１である。
流れ図ブロック８１０に示すように、通信チャネル６１８を通して伝達されたデータはＩＰＲＯＴ０機能８０４へ供給される。このデータの大部分は、最終的にはＧＰＳカルマンフィルタ８０２を行先とする。流れ図ブロック８０８に示す通信管理者機能は、ＩＰＲＯＴＯ機能から到来するデータを調整する。通信管理者機能８０８は、流れ図ブロック８１２に示すＩＣＣ機能から受信するデータも調整する。図示のようにＩｃｃ機能８１２は、データ無線機７１４（ＧＰＳ相互通信プロセッサ７２０を介して）とＧＰＳデータ収集装置７１８との間のデータ交換を行う。
【００４０】
ＧＰＳコンソール７１２は当分野においては周知である。所望の機能を提供する多くの型の装置が市販されている。このような１つの装置は、マサチュセッツ州メイナードのＤｉｇｉｔａｌ  Ｅｑｕｉｐｍｅｎｔ  ＣｏｒｐｏｒａｔｉｏｎからモデルＶＴ２２０として市販されているものである。ＧＰＳコンソール７１２は、ＧＰＳ相互通信プロセッサ７０８及びＧＰＳプロセッサ７１０に関するプロセッサ活動データを表示する。
ＧＰＳプロセッサ７１０は、ＧＰＳコンソール７２２とＧＰＳ通信インタフェースプロセッサ７２０とに結合されている。ＧＰＳコンソール７２２は当分野においては周知である。所望の機能を提供する多くの型の装置が市販されている。このような装置の１つは、マサチュセッツ州メイナードのＤｉｇｉｔａｌ  Ｅｑｕｉｐｍｅｎｔ  ＣｏｒｐｏｒａｔｉｏｎからモデルＶＴ２２０として市販されているものである。ＧＰＳコンソール７２２はユーザインタフェースとして作動し、それを通してＧＰＳプロセッサ７１０を作動させ、監視することができる。
【００４１】
ＧＰＳ通信インタフェースプロセッサ７２０は、本質的には１／Ｏボードである。これは、データ無線機７１４とＧＰＳデータ収集装置７１８とに結合されている。ＧＰＳ通信インタフェースプロセッサ７２０は、ＧＰＳプロセッサ７１０と、データ無線機７１４及びＧＰＳデータ収集装置７１８の両者との間のデータ交換を調整する。好ましい実施例のＧＰＳ通信インタフェースプロセッサ７２０は、Ｍｏｔｏｒｏｌａ  Ｉｎｃ．、Ｕ．Ｓ．Ａ．から市販されているモデルＭＶＭＥ３３１である。
データ無線機７１４は、べ一スステーション１８８に搭載され下いるデータ無線機７１４に類似したビークル１０２に搭載されているデータ無線機（図６参照）を介してＧＰＳプロセッサ７１０（ＧＰＳ通信インタフェースプロセッサ７２０を介して）間に通信リンクを確立する。好ましい実施例では、データ無線機７１４はＲＦ周波数を使用して９６００ボーで同期して通信する。べ一スステーション１８８に搭載されているデータ無線機７１４は、ビークル１０２に搭載のデータ無線機７１４に対して各衛星毎の空間バイアス及び時計バイアスの量を２Ｈｚのレート（毎秒２回）で周期的に更新する手段を提供する。べ一スステーション１８８による空間バイアス及び時計バイアスの計算に関しては後述する。
ＧＰＳデータ収集装置７１８は、卓上型コンピュータのような種々の一般的な電子処理及び記憶装置の何れかであることができる。Ｕ．Ｓ．Ａ．フロリダ州ボカレイトンのＩｎｔｅｒｎａｔｉｏｎａｌ  Ｂｕｓｉｎｅｓｓ  Ｍａｃｈｉｎｅｓ  Ｃｏｒｐｏｒａｔｉｏｎ（ＩＢＭ）製のどのパーソナルコンピュータ（ＰＣ）ででも実現できる。
【００４２】
Ｃ．移動体位置特定システム（ＭＰＳ）
好ましい実施例のＭＰＳ  ９００を図９に示す。ＭＰＳ９ＯＯは、ビークル１０２の第２位置推定を導出する、通常、この第２位置推定は第１位置推定と組合わされ、濾波されてより正確な第３位置推定が導出される。しかしながら、第１位置推定が明らかに不正確であると考えられる若干の場合には、この第２位置推定を第３位置推定として排他的に使用できる。
ＭＰＳ９００の場合、好ましい実施例は走行距離計９０２及びＩＲＵ９０４を組合せる構想である。しかし、走行距離計９０２を用いずに、ＩＲＵ９０４を使用することが可能である。走行距離計及びＩＲＵ９０４はＭＰＳ相互通信プロセッサ９０６に結合され、それによってＭＰＳ９ＯＯが構成されている。ＩＲＵ及び走行距離計は周知であり、それぞれミネソタ州ミネアポリスのＨｏｎｅｙｗｅｌｌ  Ｉｎｃ．からモデルＨＧ１０５０−ＳＲＯ１、及びイリノイ州ペオリアのＣａｔｅｒｐｉｌｌａｒ  Ｉｎｃ．から部品番号７Ｔ６３３７として市販されている。
ＩＲＵ９０４は、周知設計のリングレーザジャイロスコープ及び加速度計を具備する。好ましい実施例に使用されているＩＲＵ９０４は、航空機の位置を決定するためにボーイング７６７機に使用されているシステムの複製であるが、７６７機に対してビークル１０２が呈する小さいダイナミックス（例えば、速度）を斟酌するためにＩＲＵ９０４には変更が加えられている。
ＩＲＵ９０４は、ビークル位置を５Ｈｚで、速度を１０Ｈｚで、横揺れを５０Ｈｚで、縦揺れを５０Ｈｚで、そして偏揺れデータを５０Ｈｚで出力することができる。更に好ましい実施例では、ビークル走行距離計９０２は、ビークル１０２が走行した距離を２０Ｈｚで出力することができる。
ＩＲＵ９０４のレーザジャイロスコープを適切に機能させるためには、初めにビークル１０２の高度の推定を与えなければならない。このデータを基線位置推定として使用し、次いで所定の較正と地球１７２の回転に伴う力とを使用してジャイロスコープはビークル１０２の現位置の推定を決定する。
【００４３】
次いでＩＲＵ９０４は、この情報とＩＲＵ９０４加速度計が取得したデータとを組合せてビークルの現位置のより正確な第２位置推定を生成する。ＩＲＵ９０４からの第２位置推定及びビークル走行距離計９０２からのデータは、図９の矢印９１０及び９０８によってそれぞれ示されているように、ＭＰＳ相互通信プロセッサ９０６へ伝送される。
構成都晶の精密度の欠如のためにＩＲＵ９０４はビークル１０２の誤りの第２位置推定を供給する恐れがあることが実験により判明している。詳述すれば、好ましい実施例では動作中のＩＲＵ９０４の方向出力が、方向北から反時計方向に漂動することが観測された。この漂動はビークル１０２が、従ってＩＲＵ９０４が走行する方向に依存する。
更にこの漂動はＩＲＵ漂動方程式によって定義することができる。ＩＲＵ漂動方程式は、重み付き経路履歴技術（ＩＩ．Ｈ．）に関して説明する経路方程式の作成、または放物線バイアス技術（ＩＩ．Ｆ．２．ｂ．）に関して説明する放物線方程式の作成に類似して導出することができる。導出後、ＩＲＵ漂動方程式を使用してより正確な第２位置推定を補外することができる。
好ましい実施例では、相互通信プロセッサ１００２はＭｏｔｏｒｏｌａ  Ｉｎｃ．から市販されている６８０００マイクロプロセッサからなっている。相互通信プロセッサ１０２は、ＭＰＳ９００とＶＰＳ１ＯＯＯとの間のデータの交換を調整する。上述したような機能と同一機能を有する如何なるプロセッサも使用可能である。
【００４４】
Ｄ．ビークル位置特定システム（ＶＰＳ）
図１０に、ＶＰＳ１０００のアーキテクチャの好ましい実施例を示す。図１１はＧＰＳ処理システム７００とＭＰＳ９ＯＯとに接続されているＶＰＳ１０００の詳細図である。
ＧＰＳ処理システム７００及びＭＰＳ９００は、独立的にＶＰＳ主プロセッサ１００２に結合されている。この独立的な結合が、本発明の重要な新規特色である。これらが独立しているために、一方のシステムが障害を起こしても他方のシステムが作動不能になることはない。例えば、ＧＰＳ処理システム７００が作動しなくても、データはＭＰＳ９ＯＯによって、従ってＶＰＳ１Ｏ００によって収集され、処理される。ＧＰＳ処理システム７００及びＭＰＳ９ＯＯは、図示のように信号７１６、９０８、及び９１０をＶＰＳ主プロセッサ１Ｏ０２へ伝送する。これらの信号は、位置、速度、時刻、縦揺れ、横揺れ、偏揺れ、及び距離を含む（図７及び９、並びにそれらの説明を参照されたい）。
ＶＰＳ主プロセッサ１Ｏ０２は、ＶＰＳＩ／Ｏプロセッサ１００４に結合されている。ＶＰＳ主プロセッサ１Ｏ０２は、図示のように信号１００８をＶＰＳＩ／０プロセッサ１００４へ伝送する。この信号１００８は第３位置推定からなる。第３位置推定は上記ＧＰＳ、ＩＲＵ及び走行距離計から、具体的にはビークル１０２の第１及び第２位置推定から導出されたものである。
本発明は、矢印７１６、９０８及び９１０によって示されている信号を、ＶＰＳ主プロセッサ１００２がＧＰＳ処理システム７００及びＭＰＳ９００から受信し、それらをＶＰＳＩ／Ｏプロセッサ１Ｏ０４へ転送できるような何等かのシステム及び方法を企図している。ＶＰＳ主プロセッサ１００２はＭｏｔｏｒｏｌａ  Ｉｎｃ．、Ｕ．Ｓ．Ａ．から市販されている６８０２０マイクロプロセッサである。
【００４５】
図１２は、図１０のＶＰＳ主プロセッサ１００２の中間水準ブロック線図１２００であって、ＶＰＳカルマンフィルタ１２０２と重み付きコンバイナ１２０４とを示す、図示のように、ＧＰＳ信号７１６及び走行距離計信号９０８は重み付きコンバイナ１２００へ直接伝送される。ＩＲＵ信号９１０はＶＰＳカルマンフィルタ１２０２へ伝送される。好ましい実施例では、ＧＰＳ信号７１６は２Ｈｚのレートで伝送される。走行距離計信号９０８は２０Ｈｚのレートで伝送される。更に、第２位置推定を含むＩＲＵ信号９１０は５０Ｈｚのレートで伝送される。
ＶＰＳカルマンフィルタ１２０２は、データからの外来雑音を濾波し、処理済のデータを重み付きコンバイナ１２０４へ出力する。更にＶＰＳカルマンフィルタ１２０２は、矢印１２０８で示すように、重み付きコンバイナ１２０４からの信号を受信する。この信号はＶＰＳカルマンフィルタ１２０２を新しい位置情報でリセットするために使用される。
重み付きコンバイナ１２０４はこれらの信号を処理し、使用されているデータ収集技術の推定される精度に基づいて、各データに所定の重み付け係数を与える。即ち、好ましい実施例では、ＧＰＳ信号７１６の第１位置推定はＩＲＵ信号９１０の第２位置推定よりも重い重みが付けられる。このような重み付けをする理由は、第１位置推定はＩＲＵ９０４からの第２位置推定よりも本質的に正確だからである。
【００４６】
しかしながら、速度はＩＲＵによって決定されたものの方がより正確であり得る。従ってＩＲＵ信号９１０の速度成分は、ＧＰＳ信号７１６の速度成分よりも大きい重み付けをすることができる。本発明の好ましい実施例では、ＧＰＳ信号７１６の速度成分は排除して、ＩＲＵ信号９１０の速度成分を使用している。
重み付きコンバイナ１２０４は、２０Ｈｚで出力１２０６を生成する。出力１２０６は計算済の全てのデータを含み、２つの位置、即ち矢印１２０８で示されているようにＶＰＳカルマンフィルタ１２０２と、矢印１００８で示されているようにＶＰＳＩ／０プロセッサ１００４とへ送られる。出力１２０６はＧＰＳ衛星に関する時刻情報を含む。更に出力１２０６は、ビークルの位置、速度、偏揺れ、縦揺れ、及び横揺れに関する情報をも含む。最後に、ＶＰＳ出力１２０６がビークル１０２の第３位置推定を構成していることに注目されたい。重み付きコンバイナ１２０４からの矢印１０１８で示す別の出力は、ビークル１０２の速度に関するデータだけを含む。速度データはＶＰＳ主プロセッサ１Ｏ０２からＧＰＳ処理システム７００へ送られる。この速度データは、後述するように第１位置推定の精度を向上させるために使用される。
【００４７】
本発明は、信号７１６、９０８、及び９１０を上述の処理過程の諸段階に従ってＶＰＳ主プロセッサ１００２において処理できるような何等かのシステム及び方法を企図している。好ましい実施例では、ＶＰＳ主プロセッサ１００２は、Ｍｏｔｏｒｏｌａ  Ｉｎｃ．、Ｕ．Ｓ．Ａ．から市販されている６８０２０マイクロプロセッサである。
図１２Ａに本発明のスーパーカルマンフィルタ１２００Ａを示す。スーパーカルマンフィルタ１２００Ａは、ビークル１０２の位置推定の精度を向上させるためにデータを処理するシステム及び方法である。即ち、スーパーカルマンフィルタは第１位置推定の精度を直接的に向上させる。従って、第３位置推定の精度は間接的に向上される。好ましい実施例では、スーパーカルマンフィルタ１２００Ａは、図７のＧＰＳ処理システム７００及び図１０のＶＰＳ１０００のアーキテクチャ内のソフトウェアを構成している。スーパーカルマンフィルタ１２００Ａは、例えば集積回路、光学フィルタ等内のハードウエア内に構築することも可能である。
矢印１２１０で示すように、ＧＰＳカルマンフィルタ８０２は地上位置決定システムから、例えばＧＰＳデータ及び擬似衛星データの両方または何れか一方を含むことができる第１データを受信する。ＧＰＳカルマンフィルタ８０２はこれらのデータを操作して、矢印１００６で示すように第１位置推定（ＦＰＥ）を出力する。
矢印１０１０で示すように、ＶＰＳカルマンフィルタ１２０２は、ＭＰＳ９００からＭＰＳデータを受信する。ＶＰＳカルマンフィルタはＭＰＳデータを操作して第２位置推定（ＳＰＥ）を出力する。
重み付きコンバイナ１２０４は、矢印１００６及び１２１０で示すようにＦＰＥ及びＳＰＥをそれぞれ受信する。重み付きコンバイナ１２０４は、ビークル１０２の速度１０１８をＧＰＳカルマンフィルタ８０２へ出力する。ＧＰＳカルマンフィルタ８０２は、ビークルのビークル速度１０１８に従って矢印１００６のＦＰＥの精度を向上させるように適合させる。
ＧＰＳカルマンフィルタ８０２は、離散した時間間隔に適合するように、または連続的に適合するように設計することができる。好ましい実施例では、ＧＰＳカルマンフィルタ８０２は、価格と性能との兼ね合いから離散した時間間隔に適合させてある。
【００４８】
正確な地上位置決定システムを得るためには、１つのカルマンフィルタ（図示してない）だけを実現すればよいと考えられる。詳述すれば、第３位置推定を導出する唯一のカルマンフィルタに接続されたＧＰＳ処理システム７００及びＭＰＳ９００（走行距離計９０２及びＩＲＵ９０４の両方または何れか一方を有する）を有することが可能である。しかしながら、このような構成は好ましい実施例としての全ての好ましい特性を備えてはいないであろう。
図１２及び１２Ａのスーパーカルマンフィルタは、単一のカルマンフィルタ及び分離したカルマンフィルタの両方の都合のよい特性を有している。図示のように構成されたＧＰＳカルマンフィルタ８０２及びＶＰＳカルマンフィルタ１２０２は連続的にデータを交換し、それによって第１及び第２位置推定の制度を向上させる、従って第３位置推定の精度が向上する。ある意味で、単一のカルマン濾波システムは、第３位置推定の最終出力と入力中の位置データとの間に常駐しているのである。
別の意味では、ＧＰＳカルマンフィルタ８０２及びＶＰＳカルマンフィルタ１２０２は完全に分離した独立フィルタとして動作する。例えば、もしＧＰＳデータまたはＭＰＳデータの何れかが劣化していれば、劣化していないデータの精度に影響を与えることなく重み付きコンバイナ１２０４を介して劣化したデータの全体または一部が無視される。単一カルマンフィルタを使用するシステムでは、もしＧＰＳデータまたはＭＰＳデータの何れかが劣化していれば、最終出力即ち第３位置推定は実質的に不正確になるであろう。
【００４９】
図１０に戻って、ＶＰＳＩ／Ｏプロセッサ１Ｏ０４はＶＰＳ通信インタフェースプロセッサ１０２０に結合されている。通信インタフェースプロセッサ１０２０は、Ｍｏｔｏｒｏｌａ  Ｉｎｃ．、Ｕ．Ｓ．Ａ．から市販されているＭＶＭＥ３３１プロセッサである。以下に説明する目的と同じ目的を達成する如何なるプロセッサも使用可能である。
好ましい実施例では、ＶＰＳ通信インタフェースプロセッサ１０２０は３つの異なる装置、即ち（１）ＶＰＳコンソール１０１２、（２）データ収集装置１０１４、及び（３）航法システム１０２２に結合されている。ＶＰＳ通信インタフェースプロセッサ１０２０は出力１０１６内に含まれている第３位置推定を含むデータを上記３つの装置へ２０Ｈｚのレートで転送する。
ＶＰＳコンソール１Ｏ１２は当分野では周知であり、ミネソタ州ミネアポリスのＤｉｇｉｔａｌ  Ｅｑｕｉｐｍｅｎｔ  ＣｏｒｐｏｒａｔｉｏｎからモデルＶＴ２２０として市販されている。このＶＰＳコンソール１Ｏ１２はＶＰＳＩ／０プロセッサ１００４の現ステータスを表示するために使用される。
ＶＰＳデータ収集装置１０１４は、例えば卓上型ＰＣのような、数多く市販されている電子式処理及び記憶装置の何れかであって差し支えない。カリフォルニア州クパーティノのＡｐｐｌｅ  Ｃｏｍｐｕｔｅｒから市販されているとのＭａｃＩｎｔｏｓｈ  ＰＣもこの目的のために使用することができる。
航法システム１０２２は、ビークル１０２の航行に関連付けられた機能を備えている。ＶＰＳ１０００は第３位置推定を航法システム１０２２へ伝送するので、航法システム１０２２は自律ビークル１０２を正確且つ安全に案内することができる。
【００５０】
Ｅ．べ一スステーション
図７を参照する。べ一スステーション１８８に配置されているホスト処理システム１８６は、図７のＧＰＳ処理システム７００を具備する。べ一スステーション１８８に配置されているホスト処理システム１８６の目的は、（１）ビークル１０２の動作の監視、（２）空間バイアス（１１．Ｆ．２．の差動バイアス技術参照）の生成を可能にする地上参照点の提供、及び（３）必要に応じて他の情報を高速データ通信チャネル６１８を通してビークル１０２へ供給することである。
好ましい実施例では、べ一スステーション１８８はビークル１０２に近接して、好ましくは２０マイル以内に、配置される。この地理的に近接した関係のためにべ一スステーション１８８とビークル１０２との間に効果的な無線通信が確立される。またこれは、ビークル１０２が受信した衛星送信とベースステーション１８８が受信した衛星送信とを比較するための正確な参照点をも提供する。
正確な空間バイアスを計算するためには、地理的に近接した参照点が必要である。空間バイアス及び時計バイアスは、実質的に、ＮＡＶＳＴＡＲ  ＧＰＳ及びＧＰＳ処理システム７００に固有の共通モード雑音である。空間バイアス及び時計バイアスは一旦べースステーション１８８において計算されると、図７に示すようにデータ無線機７１４を使用してビークル１０２へ送られる。空間バイアスは、後述する種々の方法を使用して計算される。
本発明の好ましい実施例では、べ一スステーション１８８のポスト処理システム１８６はビークル１０２の自律活動をも調整し、またＶＰＳ１０００と人間の監視者とをインタフェースする。
【００５１】
Ｆ．衛星をべ一スとする精度改善
本発明は、複数の差動修正技術を介してビークル１０２の位置推定の精度を改善する。これらの差動バイアス技術は第１、第２、及び第３位置推定を向上させるために使用される。
これらの差動修正技術の幾つかは、擬似距離Ｒ０、Ｒ２、Ｒ４、及びＲ６（実際の擬似距離、及び推定擬似距離の両方）の計算内の誤差（雑音または妨害）を直接除去するように設計されている。これらの誤差を除去すると精密な第１位置推定が得られ、これはＧＰＳ処理システム７００からＶＰＳ１０００へ出力され、そして最終的にはより精密な第３位置推定がＶＰＳ１０００から航法システム１０２２へ出力される。
好ましい実施例では、べ一スステーション１８８のホスト処理システム１８６が、これらの差動技術を実行し、結果をビークル１０２へ転送する責を負っている。”差動”なる語は、べ一スステーション１８８及びビークル１０２が、独立してはいるが仮想的には同一のＧＰＳ処理システム７００を使用するから使用しているのである。更に、べ一スステーション１８８は静止し、その絶対位置が既知であるから、電子的誤差（雑音または妨害）及び誤差を誘起する他の現象を測定するための参照点として役立つ。
【００５２】
１．星座効果
図１３は本発明の好ましい実施例における第１位置推定の精度を改善するための星座効果方法の流れ図１３００である。この方法は、ビークル１０２に搭載されているＧＰＳ処理システム７００内に実現することができる。代替として、この方法をべ一スステーション１８８のホスト処理システム１８６内に実現しても差し支えない。後者の場合には、この方法によって決定された情報を爾後にビークル１０２へ伝送して第１位置推定を適切に向上せしめることになろう。
流れ図１３００は、ＧＰＳアンテナ７０２の視界内の最良衛星星座を選択する方法を示す。ビークル１０２にとっては、ＧＰＳ衛星１３２−１７０の多くがＧＰＳアンテナ７０２の視界内に存在していよう。任意数（好ましい実施例では少なくとも４個）の衛星が特定星座を形成するように、これらの衛星の部分集合だけが選択される。本質的に、地理学的な考察に基づいて“最良”または“最適”星座が選択される。ＧＰＳアンテナ及びビークルの企図された経路の視界内のＧＰＳ衛星１３２−１７０の空間内の位置が後述するようにして斟酌される。
流れ図１３００は流れ図ブロック１３０２から開始される。流れ図１３０４において、視界内の、及びＧＰＳアンテナ７０２に対する各ＧＰＳ衛星の推定擬似距離が計算される。本明細書では、推定擬似距離は、暦データと衛星からの天体暦とから導出された推定擬似距離として定義されている。暦データとは、１日の特定時刻におけるＧＰＳ衛星１３２−１７０の空間における位置を記憶している事前に記録されたデータのことである。
ＮＡＶＳＴＡＲ  ＧＰＳの場合、暦データは複数の変数を有する方程式の形状である。これらの暦方程式はＵ．Ｓ．政府から公式に入手可能である。若干の変数はＧＰＳ衛星１３２−１７０を識別する。さらなる必須入力には、推定擬似距離が決定される時刻、及び地球上の関連点の既知位置とが含まれる。
【００５３】
各ＧＰＳ衛星に関連する推定擬似距離を決定するために、以下の情報がこれらの暦方程式内に挿入される。即ち、（１）ＧＰＳ衛星を識別するパラメタ（これらはＧＰＳ衛星からのＧＰＳデータ内に符号化されている）と、（２）現在の時刻と、そして（３）べ一スステーション１８８の既知の位置である。
次に、流れ図ブロック１３０６において、極座標を使用して推定擬似距離が作図される。図１４は極座標系１４０２土の作図１４００であって、４個のＧＰＳ衛星（図示してない）のＧＰＳ衛星星座に関係付けられた推定擬似距離円１４０４、１４０６、１４０８、及び１４１０の集合を示している。推定擬似距離円１４０４、１４０６、１４０８、及び１４１０は、極図１４００の中心１４１２において交差するように描かれている。図示のようにこの座標系１４０２は、方向北からの方位を反映している。
ＧＰＳ衛星とＧＰＳアンテナとの間の相対距離も、推定擬似距離円１４０４、１４０６、１４０８、及び１４１０の寸法によって極図１４００内に表されている。即ち、例えば、推定擬似距離円１４０６によって表されているＧＰＳ衛星は、推定擬似距離円１４０８によって表されているＧＰＳ衛星より更に遠くにある。
図１４を参照する。陰影を付けた長円領域１４１２は、推定擬似距離円１４０６及び１４０８を生じさせたＧＰＳ衛星（図示してない）を考慮した時のビークル１０２の考え得る位置を示している。長円表現内の重要なパラメタは、地理学的アクセス係数比（ＧＲＡＦ）と呼ばれる長円の半長アクセスと手短アクセスとの比である。ＧＲＡＦは次の流れ図ブロック１３０８において計算する予定である。流れ図ブロック１３０８を参照する。ＧＲＡＦは長アクセスの角度と共に重み付け係数を計算するために使用され、後述するようにこの重み付け係数はＧＰＳ処理システム７００がより正確な第１位置推定を計算するのを最終的に援助する。
【００５４】
流れ図ブロック１３１２に示すように、ビークル１０２に搭載のＧＰＳ処理システム７００内のＧＰＳカルマンフィルタ８０２は、図１４に示す推定長円の形状とビークル１０２の計算された北東向きの座標とを受け入れるために変更される。更に、矢印１３１４で示すように、中心１４１２の推定位置を絶えず向上させるように上述の手順が絶えず繰り返される。流れ図ブロック１３１６において、ビークルの所望経路のための最適衛星星座が決定される。この最適星座は、ビークルの所望経路に直角な方向において最小の誤差を与えるような星座であろう。
流れ図ブロック１３１８に示すように、最適衛星星座はデータ無線機７１４を介してビークル１０２へ送信される。ビークル１０２はこの最適衛星星座を使用して第１位置推定を計算する。
【００５５】
２．差動修正技術
ａ．原形バイアス技術
図１５を参照する。流れ図１５００は当分野では周知の原形バイアス技術を示す。原形バイアス技術は、第３位置推定を限定するに当たって最終的に関与する第１位置推定の精度を向上させるために、空間バイアスを計算するための方法である。以下に説明する原形バイアス技術は、空間バイアス（原形バイアス）を決定するための参照点としてべ一スステーション１８８の既知の位置を使用する。
原形バイアス技術は、ビークル１０２に搭載のＧＰＳ処理システム７００内に実現することができる。更に原形バイアス技術は、べ一スステーション１８８に搭載のホスト処理システム１８６内に実現することもできる。後者の方式においては、好ましい実施例方法によって決定された情報が爾後にビークル１０２へ通信されて第１位置推定を適切に向上させることになろう。更に、好ましい実施例は後者の方式を採用し、べ一スステーション１８８に搭載のホスト処理システム１８６内に実現している。
図１５に示すように原形バイアス技術は、流れ図ブロック１５０２から開始される。流れ図ブロック１５０４に示すように、ＧＰＳアンテナ７０２の視界内の各ＧＰＳ衛星の実際の擬似距離（べ一ス実際の擬似距離）及び推定擬似距離（べ一ス推定擬似距離）はべ一スステーション１８８に搭載のホスト処理システム１８６内で計算される。べース実際の擬似距離はべース推定擬似距離とは独立的に計算される、べース実際の擬似距離は、ホスト処理システム１８６内のＧＰＳ受信機７０６によって計算される。更に、べ一ス推定擬似距離はＧＰＳプロセッサ７１０によって計算される。
べ一ス実際の擬似距離は、ＧＰＳ衛星（または擬似衛星）からの電磁信号の送信と、べ一スステーション１８８に搭載のホスト処理システム１８６における受信との間の伝播時間経過を測定することによって計算される。電磁信号は送信時刻を符号化している。更に、ＧＰＳ受信機７０６は受信時刻を記録する。これらの電磁信号が光速、即ち２．９９７９２４５８９８＊１０８ｍ／ｓで走行すものとすれば、各衛星の実際の擬似距離は、伝播時間経過に光速（適切な単位の）を乗ずることによって決定することができる。
【００５６】
べ一ス推定擬似距離は、（１）暦データ（ＮＡＶＳＴＡＲ  ＧＰＳでは、暦方程式）と、（２）ＧＰＳ衛星からの電磁信号の送信時刻と、（３）べ一スステーション１８８の既知の位置（べ一ス既知位置）とから計算される。送信時刻と、べ一ス既知位置（ＢＫＰ）とが暦方程式内に代入されて、ある衛星のための推定擬似距離が導出される。
流れ図ブロック１６０４に示すようにホスト処理システム１８６の回路時計と、認識されたＧＰＳ衛星の回路時計との間の時計バイアス（べ一ス時計バイアス）も計算される。好ましい実施例では、全ての衛星に関して１つのべ一ス時計バイアスが計算される。べ一ス時計バイアスは、所定の時間にわたって衛星及びホスト処理システム１８８のクロックパルスを計数することによって計算される。次いでこれらのパルスが比較され、差が求められる。次にこの差に光速、即ち２．９９８＊１０８ｍ／ｓが乗ぜられ、時計バイアスが長さの単位に変換される。しかし、本発明にはべ一ス時計バイアスを計算し、表現する如何なる方法も組入れることが可能であることを理解すべきである。
【００５７】
流れ図ブロック１５０８に示すように空間バイアス（原形バイアス）は、べ一ス実際の擬似距離から、べ一ス推定擬似距離とベース時計バイアス（長さの単位）とを差し引くことによって計算される。原形バイアスは、大気状態、受信機誤差等のような多くの異なる効果によってもたらされる。ビークル１０２の実際の位置は未知であるから、ビークル１０２を参照点として使用して原形バイアスの計算を遂行することはできないことに注目すべきである。しかし、原形バイアスの計算をビークル１０２において遂行することはできる。
流れ図ブロック１５１０に示すように、ホスト処理システム１８８内のＧＰＳカルマンフィルタ８０２は原形バイアスを用いて更新される。更に矢印１５１２で示すように、原形バイアスを計算する処理過程は連続的に遂行され、導出された原形バイアスはＧＰＳカルマンフィルタ８０２の繰り返し更新に使用される。
【００５８】
ビークル１０２はべ一スステーション１８８に近接しているから、擬似距離計算における誤差は同一と見なされる。従って、流れ図ブロック１５０８に示すようにして決定された原形バイアスも、ビークル１０２のＧＰＳ処理システム７００によって生成された実際の擬似距離を変更するために使用される。従って・流れ図ブロック１５１４に示すように、原形バイアスはデータ無線機６２０及び６２２を使用してべ一スステーション１８８からビークル１０２へ送信される。
原形バイアスはビークル１０２内のＧＰＳカルマンフィルタ８０２を更新するために使用される。ＧＰＳカルマンフィルタ８０２を更新することにより、より正確な第１位置推定が得られる。
【００５９】
ｂ．放物線バイアス技術
ＧＰＳ衛星１３２−１７０が空に昇り、沈むにつれて各ＧＰＳ衛星１３２−１７０が形成する経路を地表または地表付近において擬似距離で追尾すると放物線を辿る。従って、天空中の各ＧＰＳ衛星の経路を表す放物線関数を導出することができる。これは、好ましい実施例におけるべ一スステーション１８８に搭載のホスト処理システム１８６において遂行される放物線バイアス技術の要諦を説明している。しかし放物線バイアス技術はビークル１０２において遂行することができることに注目すべきである。図１６の流れ図１６００は放物線バイアス技術を示す。放物線関数（モデル）は、べースステーション１８８においてＧＰＳアンテナ７０２の視界内の各ＧＰＳ衛星毎に計算される。
流れ図１６００は流れ図ブロック１６０２から開始される。流れ図ブロック１６０４に示すように、時刻ｔ（ｎ）に、前述のようにＧＰＳ受信機７０６を使用してＧＰＳアンテナ７０２の視界内の各ＧＰＳ衛星毎に実際の擬似距離がべ一スステーション１８８において決定される。流れ図ブロック１６０６に示すように、これらの（各ＧＰＳ衛星毎の）実際の擬似距離は各ＧＰＳ衛星毎の放物線最良適合モデル内に組入れられる。即ち、流れ図ブロック１６０６において、各ＧＰＳ衛星毎の放物線モデルに１つの点が付加される。
流れ図ブロック１６０８に示すように、各ＧＰＳ衛星毎の放物線関数を推定するために十分な点が放物線モデル上で決定されたか否かの試験が行われる。収集された点の数は特定の統計Ｒ２値を決定する。好ましい実施例では、Ｒ２値は次のようにして計算される。
【００６０】
Ｒ２＝和２（推定擬似距離（ｔ）一推定擬似距離の平均）／和２（実際の擬似距離（ｔ）一実際の擬似距離の平均）
上記標準統計方程式は当分野では公知である。この方程式に関するさらなる考察はＤｒａｐｅｒ，  Ａｐｐｌｉｅｄ  Ｒｅｇｒｅｓｓｉｏｎ  Ａｎａｌｙｓｉｓ、１９６６  ｅｄｉｔｉｏｎを参照されたい。Ｎを計算された推定及び実際の両擬似距離の数を定義することによって、及び方程式を数学的に展開することによって、より使用可能な形状の以下の方程式を容易に導出することができる。
Ｒ２＝Ｎ＊和（全推定擬似距離の自乗）一２＊和（推定擬似距離）＊和（実際の擬似距離）十和（実際の擬似距離）２／Ｎ＊和（全実際の擬似距離の自乗）一和（実際の擬似距離）
流れ図ブロック１６０８に示すように、好ましい実施例では、もしこのＲ２値がＯ．９８よりも大きければ、この放物線モデルはＧＰＳ衛星の将来経路を推定するのに十分に正確であるものと見なされる。もしＲ２値が０．９８よりも小さいかまたは等しければ、放物線モデル上のさらなる点を計算しなければならない。これらの点は、ＧＰＳ受信機７０６によって連続的に計算されている擬似距離データを組入れることによって計算される。
流れ図ブロック１６１０に示すようにＮ値がインクリメントされ、流れ図ブロック１６０４に示す擬似距離を計算する時刻が増加したことを示す。ＧＰＳ受信機７０６は各ＧＰＳ衛星毎の実際の擬似距離を２Ｈｚで（１秒間に２回）出力するから、各ＮインクリメントはほぼＯ．５秒を表しているべきである。
【００６１】
Ｒ2値を0.98よりも大きくするような十分なデータが収集されていれば、流れ図ブロック1612に示すように、この放物線モデルは各衛星軌道経路を表すのに十分に正確であると見做される。流れ図ブロック1614に示すように、これで放物線モデルは完壁であり、モデル上の将来点を補外することができる。
流れ図ブロック1614に示すように、時刻Ｔ(n+1)における各放物線モデル上の軌跡点が計算される。軌跡点とは、時刻Ｔ(n+1)におけるGPS衛星の予測される実際の擬似距離のことである。一旦この軌跡点が計算されれば、流れ図ブロック1616に示すように、その軌跡点に関する距離(GPSアンテナ702とGPS衛星との間の距離)が計算される。
【００６２】
流れ図ブロック１６１８において、好ましい実施例では現時刻である時刻Ｔ（ｎ＋１）における実際の擬似距離が計算される。これらの実際の擬似距離は前述のようにＧＰＳ受信機７０６によって計算される。時刻Ｔ（ｎ＋１）におけるこれらの実際の擬似距離は、流れ図ブロック１６００の次の繰り返しにおいて放物線最良適合モデル内に組入れられる。
流れ図ブロック１６２０に示すように、時刻Ｔ（ｎ＋１）に計算された各衛星毎の実際の擬似距離及びべ一ス時計バイアス（長さの単位）が、軌跡点距離から差し引かれて各衛星毎の放物線バイアスが生成される。
次いで流れ図ブロック１６２４に示すように、これらの放物線バイアスはデータ無線機７１４を介してビークル１０２のＧＰＳ処理システム７００へ転送される。ビークル１０２のＧＰＳ処理システム７００はこれらの放物線バイアスを使用して実際の擬似距離（ビークル実際の擬似距離）の精度を向上させ、それによって第１位置推定の精度を向上させる。
【００６３】
Ｃ．べ一ス残留バイアス技術
図１７にべ一ス残留バイアス技術を実現するための流れ図１７００を示す。好ましい実施例では、べ一ス残留バイアス技術はべ一スステーション１８８に搭載されているホスト処理システム１８６において遂行される、べ一ス残留バイアスは、べ一スステーション１８８において計算された後にビークル１０２のＧＰＳ処理システム７００へ転送される。ビークル１０２のＧＰＳ処理システム７００はこのべ一ス残留バイアスを使用して第１位置推定の精度を向上させる。
【００６４】
本明細書ではべ一ス残留バイアスとは、べ一スステーション１８８のべ一ス既知位置とベースステーション１８８の位置推定（もしビークル１０２によって計算されれば、第１位置推定）との差であり、これはべ一スステーション１８８のホスト処理システム１８６によって計算される。これが如何に機能するかを説明するために、べ一スステーション１８８がメープル通りとエルム通りの角に位置しているものとする。またべ一スステーション１８８のＧＰＳ処理システム７００は、べ一スステーション１８８がべ一ス既知位置（エルム通りとメーブル通りとの角）の真南４マイルの処にあると推定しているものとする。べ一ス残留バイアスが、真南の方向４マイルに等しい距離であることは明白である。
ビークル１０２のＧＰＳ処理システム７００はべ一スステーション１８８のＧＰＳ処理システム７００と同一であるから、計算内のこの４マイル誤差はビークル１０２及びべースステーション１８８において発生するものと考えることができる。そこでビークル１０２はこの情報をそのＧＰＳプロセッサ７１０において使用することができる。実際に、ビークル１０２のＧＰＳプロセッサはデータ内の４マイル真南の誤差を斟酌してその第１位置推定を変更する。
【００６５】
以下に図１７を参照してべ一ス残留バイアス技術の原理体系の詳細を説明する。流れ図ブロック１７０４において、べ一スステーション１８８の正確な極座標ｘ０、ｙ０、ｚＯがべ一ス既知位置から得られる。
流れ図ブロック１７０６において、べース実際の擬似距離、べース推定擬似距離、及びべ一ス時計バイアスがべ一スステーション１８８のホスト処理システム１８６によって計算される。もしビークル１０２のＧＰＳ受信機７０６がＧＰＳ衛星の特定星座（図示してない）からのデータを読取るように構成されていれば、べ一スステーション１８８のＧＰＳ受信機７０６は同一衛星星座を使用することになろう。
流れ図ブロック１７０８に示すように、べ一スステーション１８８の位置推定（べ一ス位置推定）が計算される。好ましい実施例では、このべ一ス位置推定はビークル１０２における第１位置推定と同一の方法で計算される。
次に流れ図ブロック１７１０において、べ一ス位置推定がべ一ス既知位置と比較される。べース位置推定とベース既知位置（エルム通りとメープル通りとの角）との差（例えば上例では４マイル）を（もしあれば）、本明細書ではべ一ス残留バイアスと呼ぶ。
流れ図ブロック１７１２に示すように、べース残留バイアスはデータ無線機７１４によってビークル１０２へ送信される。べース残留バイアスは、ビークル１０２のＧＰＳプロセッサ７１０において処理されて第１位置推定の精度が向上される。
【００６６】
ｄ．べ一ス相関器バイアス技術
図１７Ａは、ビークル１０２の第１位置推定の精度を改善するために本発明に使用されるべ一ス相関器技術の高水準流れ図１７００Ａである。要約すれば、本技術は参照点の既知の位置を精度向上の方策として使用する。好ましい実施例では、べースステーション１８８を参照点として働かせる。以下に図６を参照して流れ図１７００Ａの原理を説明する。
べース相関器技術では、流れ図ブロック１７０５に示すように、先ず空間バイアス（べース空間バイアス）及び時計バイアス（べース時計バイアス）が図６のべースステーション１８８のホスト処理システム１８６によって計算される。べース空間バイアスは、上述の原形バイアス及び放物線バイアスを含む、しかしこれらに限定されない何等かの空間誤差計算であることができる。
即ち原形バイアスは、実際の擬似距離（べース実際の擬似距離）から推定擬似距離（べース推定擬似距離）及びべ一ス時計バイアスを差し引くことによって計算されることを思い出されたい。べース推定擬似距離は、（１）暦データと、（２）衛星信号の送信の時刻と、（３）べースステーション１８８の既知位置（べース既知位置）とから決定される。べース時計バイアスは、ＧＰＳ衛星及び擬似衛星の両方または何れか一方の送信回路と、べースステーション１８８の受信回路との時計時刻の差である。べース時計バイアスは、それらに光速を乗ずることによって長さの単位で表される。べース実際の擬似距離は、ＧＰＳ衛星及び擬似衛星の両方または何れか一方からべースステーション１８８へ送られる電磁信号の送信と受信との間の伝播時間遅延から決定される。
【００６７】
更に、放物線バイアスは、観測された各ＧＰＳ衛星のべース実際の擬似距離のための放物線モデルを作成し、これらの放物線モデルから値を補外することによって計算される、好ましい実施例では放物線バイアスは、べース実際の擬似距離、マイナス作成された放物線モデルから補外された値、マイナスペース時計バイアス（長さの単位）である。
流れ図ブロック１７０９に示すように、べ一スステーション１８８は通信チャネル６１８を通してビークル１０２へ、べ一ス実際の擬似距離と、べ一ス推定擬似距離と、べ一ス空間バイアスと、べ一ス時計バイアスと、べ一スステーション１８８のべ一ス既知位置とを転送する。推定自体を極めて正確にするために、べ一ス既知位置は、本発明の新規なシステム及び方法、または何等かの普通のシステム及び方法を含む、しかしこれらに限定されることがない適切な手段によって決定することができる。ビークル１０２が上記情報をべ一スステーション１８８から受信した後に、ビークル１０２のＧＰＳプロセッサ７１０はこの情報を、それ自身の空間バイアス（ビークル空間バイアス）の計算に使用する。
【００６８】
ビークル１０２が流れ図ブロック１７１３においてビークル空間バイアスを導出するための計算を遂行する前にそのＧＰＳ受信機７０６は、それ自身の実際の擬似距離（ビークル実際の擬似距離）と、それ自身の推定擬似距離（ビークル推定擬似距離）と、それ自身の時計バイアス（ビークル時計バイアス）とを計算する。ビークルＧＰＳプロセッサ７１０はビークル実際の擬似距離から、ビークル推定擬似距離と、ビークル時計バイアスと、流れ図ブロック１７０９においてべ一スステーション１８８から送られたべ一ス空間バイアスとを差し引く。その結果がビークル１０２におけるビークル空間バイアスのより正確な計算である。次いで流れ図ブロック１７１７に示すように、ビークル空間バイアスを使用してビークル１０２の第１位置推定（ＦＰＥ）をより正確に変更する。ＦＰＥがビークル１０２の絶対位置（地球１７２の中心に対する）の推定であることに注意すべきである。
流れ図ブロック１７２１から始めて、ビークル１０２のＦＰＥを改善するために繰り返し方法が設けられている。この方法は、べ一スステーションを一種の相関器として使用することを企図している。好ましい実施例では、この方法をＧＰＳカルマンフィルタ８０２によって実現している。
【００６９】
流れ図ブロック１７２１において、ビークル１０２に対するべ一スステーション１８８の推定相対位置（ＨＢＥ）が決定される。ＦＰＥの初期状態は、ｉを繰り返しに対応する正の整数値として、ＦＰＥ（ｉ）の現在の値を取る。従って、この方法が流れ図ブロック１７１７からブロック１７２１へ進むと、ＦＰＥ（ｉ）の現在の値はＦＰＥ（０）になる。
更に流れ図ブロック１７２５において、ビークル１０２は次に、べ一スステーション１８８からビークル１０２に転送されたべ一ス実際の擬似距離と、べ一ス推定擬似距離と、べ一ス空間バイアスと、べ一ス時計バイアスとを使用して、べ一スステーション１８８の推定位置（べ一ス推定位置；ＢＥＰ）を計算する。このＢＥＰが絶対位置（地球１７２の中心に対する）であることに注意すべきである。ＦＰＥからＢＥＰを差し引くことによって、ビークル１０２に対するべ一スステーション１８８の推定相対位置（ＨＢＥ）が決定される。
流れ図ブロック１７２５に示すように、ＨＢＡが決定される、ＨＢＡはビークル１０２に対するべ一スステーション１８８の別の推定相対位置である。しかしＨＢＥとは異なり、ＨＢＡはＦＰＥからべ一ス既知位置（ＢＫＰ）を差し引くことによって計算される。従ってＨＢＥとＨＢＡとは、前者がＧＰＳデータ及び疑似衛星データの両方または何れか一方を使用して計算されるのに対して、後者は既知データを使用して計算される点が異なる。
【００７０】
次に流れ図ブロック１７２９において、ＨＢＥからＨＢＡを減算することによってオフセットが計算される。好ましい実施例では、このオフセットは、二次元直交座標系内のベクトルである。ビークル１０２とベースステ一ション１８８との高度差を斟酌する場合には三次元ベクトルを実現できることも考えられる。
流れ図ブロック１７３３において、古いＦＰＥからオフセットを差し引くことによって新しいＦＰＥ（ｉ）が計算される、換言すれば、オフセットはバイアスとして使用され、ＦＰＥ（ｉ）の精度を向上させるためにＦＰＥ（ｉ）から差し引かれる。
流れ図ブロック１７３７において、オフセットが所定のしきい値と比較される。好ましい実施例では、各ベクトル成分は対応するしきい値を有している。もし全ての成分がそれらの対応する所定のしきい値よりも小さくなければ、戻り矢印１７３９で示すように流れ図１７００Ａは再び流れ図ブロック１７２１を開始する、この場合正の整数ｉが１だけ増加され、その繰り返しが別の繰り返しであり、異なるＦＰＥ（ｉ）であることを指示する。本発明は所定のしきい値に達するか、またはそれを超えるまで循環的に、またはループ状に動作する。
オフセットが最終的に所定のしきい値に達すると、流れ図ブロック１７４３に示すようにＦＰＥはＦＰＥ（ｉ）の現状態を取る。以上のように、べ一ス相関器バイアス技術は高精度のＦＰＥをもたらす。
【００７１】
Ｇ．衛星位置予測装置
本発明は、べ一スステーション１８８及びビークル１０２の両方または何れか一方の既知の絶対位置に対するＧＰＳ衛星１３２−１７０の将来位置を予測することができる。将来位置は、ホスト処理システム１８８及びＶＰＳ１０００の両方または何れか一方のＧＰＳプロセッサ７１０によって計算された推定擬似距離に基づく。更に、この計算はべ一スステーション１８８及びビークル１０２の両方または何れか一方において遂行し、必要ならば、何処へでも転送することができる。
ＧＰＳ衛星１３２−１７０の将来位置を予測することによって、十分事前にビークル１０２のための最適衛星星座を決定することができる。従って本発明は、衛星の可用性及び非可用性を体系的に予測することができる。また本発明によればビークル１０２の動作、サービス、及び保守に関する将来計画が可能になる。
【００７２】
図１８の流れ図１８００は本発明の星座位置予測方法を示す。流れ図ブロック１８０４において、特定のＧＰＳ衛星に関して、今までには示さなかった多くの理由の何れかのために、将来データ及び時刻が入手または選択される。
流れ図ブロック１８０６に示すように、将来データ及び時刻を取得した後に、べ一スステーション１８８及びビークル１０２の両方または何れか一方の位置が決定される。好ましい実施例では、べ一スステーション１８８を参照点として使用する。べ一スステーション１８８の位置はべ一ス既知位置またはべ一ス位置推定（両者共べ一ス残留技術に関して説明済）であってよい。好ましい実施例ではベース既知位置を使用し、以下の説明はこれに基づく。
次に流れ図ブロック１８０８に示すように、暦データを参照する。本明細書の初めの部分で説明したように、ＮＡＶＳＴＡＲ  ＧＰＳの暦データは暦方程式の形状である。衛星の識別と、将来データ及び時刻と、べ一ス既知位置とを暦方程式内に入力することによって、どの衛星の将来位置も決定することができる。
【００７３】
暦方程式を使用してべ一スステーション１８８に対する衛星の将来位置を決定する場合の将来位置は、流れ図ブロック１８０８に示すように直交ＸＹＺ座標である。最後に、好ましい実施例では流れ図ブロック１８１０において、ＸＹＺ座標とベースステーション１８８の位置とから衛星の緯度、経度、高度及び方位が計算される。
衛星の将来位置の計算から、最適衛星星座を決定することができる。べ一スステーション１８８を参照点として使用して決定された最適衛星星座は、もしビークル１０２がべ一スステーション１８８に近接していれば、ビークル１０２へ転送することができる。
【００７４】
Ｈ．重み付き経路履歴
本発明の重み付き経路履歴技術は、ＧＰＳ処理システム７００から導出するビークル１０２の第１位置推定の精度を改善する。重み付き経路履歴技術は、ＶＰＳ１０００によって導出する第３位置推定の精度を改善するために以下に記述するのと同じようにして実現することができる。重み付き経路履歴技術を図１９及び２０に示す。
本質的に重み付き経路履歴技術は、将来の第１位置推定の有効性を試験するために、先行第１位置推定を使用してビークル経路モデルを導出する。重み付き経路履歴技術を使用することによって、第１位置推定の漂動が減少し、偽の位置計算を行わない免除特性が向上する、本明細書で使用する“漂動”とは、ＧＰＳ処理システム７００が、ビークル１０２の実際の経路からずれた誤りのビークル位置を推定する傾向のことである。
【００７５】
図１９を参照する。重み付き経路履歴流れ図は流れ図ブロック１９０２から開始される。流れ図ブロック１９０４に示すように、ビークル１０２の第１位置推定がＧＰＳ処理システム７００によって計算され、記録される。第１位置推定は時刻に関連して記録される。図２０に示すように、ビークル１０２の第１位置推定２００２、２００４、２００６、２００８、２０１０、及び２０１２は図２０に作図され、最終的にビークル経路２０２２を導出する。
流れ図ブロック１９０６においては、第１位置推定を使用してビークル１０２の経路に最良適合する経路方程式が処理／導出される。換言すれば、第１位置推定が時間に関して累積され、正確な“経路方程式”が導出されるのである。好ましい実施例では、経路方程式は二次（放物線）方程式である。しかし、曲折するビークル経路及びビークル転回に関して、三次（数学的変曲を有する）方程式も含まれることを理解すべきである。更に、本発明の実施例は、無限数の異なるビークル経路を写図するために、どのような型の方程式の組合わせをも使用することができる。
流れ図ブロック１９０８において、経路方程式及び第１位置推定に関する統計Ｒ２値が計算され、数値的しきい値と比較される。好ましい実施例では、このしきい値は０．９８に設定されている。統計Ｒ２値に関しては本明細書において既に説明済である。図１９の重み付き経路履歴技術に関しては、Ｒ２値は、これまでに求められた第１位置推定の数を反映し、従って経路方程式からの将来予測の統計的精度を表す。
【００７６】
もしＲ２値が０．９８より大きいか、または等しければ、流れ図ブロック１９１０において新経路方程式を導出すべきか否かを決定する試験が遂行される。換言すれば、現在収集されている第１位置推定並びに経路方程式が不正確であり、従って頼るべきではないか否かの決定が行われるのである。
好ましい実施例では、第１位置推定の数が計数され、しきい値２０と比較される。どのようなしきい値数を事前選択しても差し支えない。もし２０より多くの第１位置推定を計数すれば、流れ図はブロック１９１４へ移動する。流れ図ブロック１９１４は、流れ図１９００の次の繰り返し中に、新しい経路方程式を流れ図ブロック１９０６から開始されることを指示する。
もし２０より少ないか、または等しい第１位置推定が計算され、収集されていれば、流れ図ブロック１９０６の現経路方程式はそのまま使用され、流れ図１９００の次の繰り返し中に再び検討される。更に、流れ図ブロック１９１２に示すように第１位置推定がＧＰＳ処理システム７００によって出力される。
【００７７】
図２０に検討中のシナリオを図式的に示す。ビークル１０２の第１位置推定２０１０は、経路方程式の最良適合予測２００６とは極端に異なっている。従って経路方程式のＲ２値が所定のしきい値より大きいか、または等しい限り、及び十分な位置推定がサンプルされている限り、第１位置推定２０１０は最良適合予測２００６によって置換される。
線２０１４及び２０１６は、第１位置推定の許容範囲を示す。これらの線２０１４及び２０１６は、Ｒ２値の物理的表示を表す。従って、ＧＰＳ処理システム７００は、線２０１６の広がりの外側の第１位置推定２０１０の代わりに、最良適合予測２００６を航法システム１０２２へ出力する。
図２０Ａは、図１９及び２０で説明した重み付き経路履歴技術を実現する方法の高水準流れ図２０００Ａである。図示のようにこの方法は、鋭い角、交差、及び何等かの極端な非線形経路の全てまたは何れかを有するビークル走行経路を受け入れる。この方法は、ＧＰＳ処理システム７００が出力するビークル１０２の第１位置推定（ＦＰＥ）の精度を向上させる。
【００７８】
好ましい実施例は、ソフトウェアによって図２０Ａの新規な原理体系を実現する。このソフトウェアは、ビークル１０２及びべ一スステーション１８８の両方または何れか一方のＧＰＳ処理システム７００のＧＰＳプロセッサ７１０内に位置させることができる。
流れ図１６００Ａは、流れ図ブロック２００１から開始され、流れ図ブロック２０１９で終る。流れ図ブロック２００５に示すように、図７及び８で説明したＧＰＳ処理システム７００は、本明細書で説明済のバイアス技術の何れかを使用して第１位置推定を計算する。好ましい実施例では、図２０Ａの方法に採用されるバイアス技術は、例えば図１５の原形バイアス技術及び図１６の放物線バイアス技術を含む。
【００７９】
流れ図ブロック２００９において、ビークル１０２が鋭い角、交差、または不規則な経路に接近中か、またはその中にあるか否かの決定が行われる。この質問に答えるために必要な情報は、図４のナビゲータ４０６からＧＰＳプロセッサ７１０に供給することができる。もしこの質問に対する答えが否であれば、流れ図２０００Ａは矢印２０１３で示すように進められる。そうでなければ、即ちもしこの質問への答えが肯定ならば、流れ図２０００Ａは矢印２０２１で示すように進む。これらの両進路に関して以下に説明する。
ビークル１０２が極端な非線形経路に接近中でも、その中に位置してもいなければ、流れ図２０００Ａは流れ図ブロック２０１５を開始する。流れ図ブロック２０１５においてＧＰＳプロセッサ７１０は、１またはそれ以上のバイアス技術を使用して導出した第１位置推定をＶＰＳ１０００へ出力する。図１０及び１１において説明済のＶＰＳ１０００が、ＧＰＳ処理システム７００から送られた第１位置推定を部分的に使用してビークル１０２の第３位置推定を計算することを思い出されたい。
【００８０】
ビークル１０２が極端な非線形経路に接近中である場合には、流れ図２０００Ａは流れ図ブロック２０２３を開始する。流れ図ブロック２０２３では、最終的により線形な経路が後続するまで、バイアス技術は一時的に放棄される。流れ図ブロック２０２７に示すように、ＧＰＳプロセッサ７１０はバイアス技術に無関係にビークル１０２の第１位置推定を計算する。
次に流れ図は流れ図ブロック２０３１へ進む。ビークル１０２が比較的線形の経路に接近中か、またはその中にあるか否かの決定が行われる。もし肯定であれば、流れ図２０００Ａは戻り矢印２０３３によって示されているように流れ図ブロック２００５へ戻される。流れ図ブロック２００５においては、先に終了させられた何れかのバイアス技術が再び使用される。
【００８１】
図１６の放物線バイアス技術の場合には、観測される各ＧＰＳ衛星毎に新しい最良適合放物線モデルが作成される。観測される各ＧＰＳ衛星毎に実際の擬似距離がある時間周期に対して決定され、各ＧＰＳ衛星毎の放物線モデルが作成されることを思い出されたい。放物線モデルの精度があるしきい値より大きくなるまでは、これらのモデルは使用されない。本発明では、統計Ｒ２値がＯ．９９より大きくなるまでは放物線モデルは使用されない。
一方、もしビークル１０２が比較的線形の経路に接近中でもなく、またその中に位置してもいなければ、流れ図２０００Ａは前述の流れ図ブロック２０１５へ移動する。しかしこの点でＶＰＳ１０００へ送信される第１位置推定が如何なるバイアス技術も使用されずに導出されたものであることに注目すべきである。
【００８２】
Ｉ．反選択可用性
Ｕ．Ｓ．政府（ＮＡＶＳＴＡＲ  ＧＰＳの運用者）が、時々、時計パラメタ及び天体暦パラメタの両方または何れか一方を変更することによって、ＧＰＳ衛星１３２１７０から送信されるＧＰＳデータ内に誤差を導入させるものと信じられる。換言すれば、Ｕ．Ｓ．政府はＧＰＳデータの可用性を選択的に変更できるのである。例えば、このような操作は国家非常時に行われるであろう。それでもＵ．Ｓ．政府はＮＡＶＳＴＡＲ  ＧＰＳを使用できるのである。何故ならば、Ｕ．Ｓ．政府はＰモードと呼ばれる他の異質の型の擬似ランダムコードを使用するからである。従って、Ｕ．Ｓ．政府はＣ／Ａモードを弱体化することができる。このような弱体化によって、ＧＰＳ受信機７０６は不正確な実際の、及び推定擬似距離を、従って不正確な第１位置推定を計算してしまうようになる。本発明の反選択可用隼技術は、ＧＰＳデータの何等かの誤導入を検出し、補償する方策である。
【００８３】
図２１に、反選択可用性技術の流れ図２１００を示す。好ましい実施例では、反選択可用性技術はホスト処理システム１８６のＧＰＳプロセッサ７１０内で遂行されている。しかしこの技術は、ビークル１０２のＧＰＳプロセッサ７１０内で実現することが可能である。流れ図２１００は、流れ図ブロック２１０２で開始され、流れ図ブロック２１１８で終了する。
流れ図ブロック２１０４において、ＧＰＳアンテナ７０２の視界内のＧＰＳ衛星の推定擬似距離（予測推定擬似距離；Ｏｉｊ）が、古い暦データを使用することによって予測される。古い暦データとは、先にＧＰＳ受信機７０６によって記録され、またＧＰＳプロセッサ７１０が現在受信したＧＰＳデータには関わりなく予測推定擬似距離を計算するのを可能にするＧＰＳデータ、またはその一部のことである。ある意味では、この古い暦データは現在受信したＧＰＳデータの完全性を調べるのに使用されるのである。好ましい実施例では、この古い暦データはＧＰＳ受信機７０６によって受信された先行天体暦である。
更に流れ図ブロック２１０４において、ＧＰＳ衛星によって送信中の現天体暦データ（ＧＰＳデータの部分集合）と、べ一スステーション１８８のべ一ス既知位置とを使用して、ＧＰＳ衛星の現推定擬似距離（“Ｎｉｊ”）が通常のように計算される。
流れ図ブロック２１０６において、予測推定擬似距離（暦を使用）と、現推定擬似距離（最新天体暦を使用）とが比較される。流れ図ブロック２１０６に示すように、予測推定擬似距離と現推定擬似距離のユークリッドノルムが計算され、所定のしきい値と比較される。
【００８４】
もしユークリッドノルムが所定のしきい値よりも大きければ、流れ図ブロック２１０８に示すように天体暦データは信頼できないものとされる。従って、流れ図ブロック２１０８に示すように、べ一スステーション１８８の位置推定を計算する代わりに最新の有効暦データが使用される。次いで流れ図２１００は流れ図ブロック２１１０へ進められる。
もしユークリッドノルムが所定のしきい値よりも小さければ、その天体暦データは適切であるものとされ、流れ図２１００は流れ図ブロック２１１０へ進められる。
次に流れ図ブロック２１１０に示すように、現時刻と、現在受信したＧＰＳデータか、または古い暦データの何れか（流れ図ブロック２１０６において決定）とを使用してべ一スステーション１８８のべ一ス位置推定が計算される。
流れ図ブロック２１１２においてこのべ一ス位置推定が予測値と比較される。換言すれば、べ一スステーション１８８の所在（べ一ス既知位置）は既知であるから、反選択可用性技術を使用したべ一ス位置推定の精度は、所定のしきい値に対して容易に調べることができるのである。
もし精度が所定のしきい値内にあれば、流れ図ブロック２１１６に示すようにそのＧＰＳデータが適切であるという指示がビークル１０２へ送られる。その結果べ一スステーション１８８は第１位置推定を計算するのに必要な情報をビークル１０２へ転送する。転送される情報は、例えばべ一ス時計バイアス、空間バイアス（原形バイアス、放物線バイアス、べ一ス残留バイアス）、べ一ス推定擬似距離、及びべ一ス実際の擬似距離の全てまたは何れかを含むことができる。
【００８５】
もし計算されたべ一スステーション１８８が所定のしきい値内になければ、流れ図ブロック２１１４に示すように、推定べ一ス位置が所定のしきい値内に入るようにべ一ス時計バイアス及びべ一ス空間バイアスの両方または何れか一方が操作される。そこで、流れ図ブロック２１１６に示すようにべ一ス推定位置を受け入れ可能なしきい値内に移動させるのに必要なべ一ス時計バイアスがビークル１０２へ送られるのである。
【００８６】
Ｊ．探査
ビークル１０２の位置推定及び航路の決定に加えて、本発明は地球１７２の表面の探査を実時間で達成する別の実施例に使用することができる。即ち、本発明の技術及び方法を使用して地球１７２上の如何なる点の位置も計算することができる。
Ｋ．図形表現
本発明は、ホスト処理システム１８８のユーザインタフェース（図示してない）上に図形画像を生成をさせる。図形画像によって、べ一スステーション１８８に搭乗するユーザは、ビークル１０２、及び本発明を使用して航行中の任意の他のビークルの経路を視ることができる。好ましい実施例では、もし望むならば、図形画像を市販のビデオ表示装置上に表示させ、画面を普通のプリンタによって印刷させることができる。
【００８７】
ＩＶ．航法システム
Ａ．概要
自律航法システムの実現を検討する際に、点Ａから点Ｂまでの航行を成功させるためには、自律システムが答えることができなければならない若干の基本的な質問が存在する。最初の質問は、“今我々（ビークル）は何処にいるのか？”である、この最初の質問には、区分ＩＩ．において説明した本発明の位置特定システム部分が答える。
次の、第２の質問は、“我々は何処へ行くのか、そしてどのようにしてそこへ行くのか？”である。この第２の質問は、本区分（ＩＩＩ．）において説明する本発明の航法システム部分の領域に入る。
さらなる（第３の）質問は、“どのようにして我々はビークルを実際に物理的に運動させるのか、そのようにするためには例えばどのアクチュエータが含まれるのか（舵取り、速度、制動、等々）？”である。これも以下に説明する航法システムのビークル制御サブシステムの領域である。
上述のように、例えば採鉱用ビークルの自律航行は、従来の航行よりも優れた若干の長所を提供する。就中、優れた生産性を提供するのはビークルの終日、即ち２４時間運転である。危険な作業環境、または低視程の作業環境が提示する諸問題は、自律システムが解決するのに特に適した問題である。
例えば、低視程のために年間２００日は作業不可能な採鉱現場が存在する。産業公害または核公害によって汚染されているために人の生命にとって危険であるかも知れない他の領域も存在する。ある領域は、人間がそこで働くには極めて苛酷であるか、または実行不能な程遠方、または荒廃しているかも知れない。本発明の応用は、地球外動作、例えば必要ＧＰＳ衛星が月の軌道内に配置されていることを条件に、月における採鉱を予見的に含むことができる。
【００８８】
図３に示すように、本発明の典型的な応用は採鉱現場における採鉱車の航行に関し、この現場には基本的な３つの作業領域、即ち積荷現場、輸送路、及びダンプ現場が存在する。積荷現場では、運搬車は種々の方法の何れかで、例えば直接制御または遠隔制御の何れかによって制御される手動ショベル、または自律ショベルによって鉱石を積荷することができる。次いで運搬車は輸送路と呼ばれる領域を走行しなければならない。輸送路は数百メートル程度しかない場合も、数キロメートルに達する場合もある。輸送路の終りがダンプ現場であり、鉱石は例えば粉砕、またはその他に精製するために運搬車からダンプされる。本発明は、運搬車を輸送路に沿って制御するために自律位置特定及び航法を使用する。自律航行する燃料補給ビークル及び保守ビークルも考えられる。
図４及び５を参照する。ＡＭＴ（自律採鉱トラック）の航行は、幾つかのシステム、装置及び機能の全てまたは何れかを包含する。上記総合ＡＭＴシステムのサブシステムＶＰＳ１０００は、例えばビークルが北及び東位置を含む何処に位置しているかを指示する位置データを出力する。
【００８９】
図４及び５を参照する。ＶＰＳからのデータ出力はナビゲータ４０６によって受信される。ナビゲータは、ビークルが何処へ行きたいのか（ルートデータから）、及びどのようにしてそこへ到達するかを決定し、舵取り及び速度命令からなるデータをビークル制御機能ブロック４０８へ出力してビークルを運動させる。
次いでビークル制御ブロックは低水準命令を、ガバナ、ブレーキ、及び伝導装置のような種々のビークル１０２システムヘ出力する。ビークルが行先へ向かって運動すると、ビークル制御フロック及びＶＰＳは、例えばビークルのシステムの障害状態、現速度等々を表すフィードバック情報をビークルから受信する。
航法は予期せざる障害物に対処する障害物処理（検出及び回避）能力をも含んでいなければならない、走査システム４０４は、ビークルの予定の道筋内の障害物、及び側から接近するかも知れない障害物を検出して、これらのナビゲータに通知する。
次いでナビゲータは、障害物を回避するための動作が必要か否かを決定することを要求されるかも知れない。もし動作が要求されれば、ナビゲータはどのようにして障害物を回避するかを決定する。またナビゲータは、障害物を回避した後に、どのようにしてビークルをその行先に向かう経路に戻すかを決定する。
脈絡図と名付けた図３５、及び図３７Ａ−３７Ｄを参照する。数字入りの円で示されている定義を以下に示す。
【００９０】
５０２．ポスト命令＆照会：
ホストによってビークル管理者に与えられる命令。これらの命令は幾つかの型であり得る。
開始／終了；
供給パラメタ；
緊急時動作；及び
指令。
照会はナビゲータの種々の部分のステータスに関して問合わせる。
【００９１】
５０４．ホストヘの返答：
これらは、ホストによってなされた照会に対する応答である。
４３２．位置データ：
これは、ＶＰＳによって供給されるストリーム化された情報である。
４１６．距離データ：
これは、線レーザスキャナからの距離データである。
４３２．  ＶＰＳ制御：
これらは、ＶＰＳの始動、停止、及びモード間の切り換えを行うために与えられる命令である。
４１６．スキャナ制御：
これらは、レーザスキャナに運動を開始させ、速度パターンを追随させるように設定するために送られる命令である。
【００９２】
４２０．舵取り＆速度命令：
これらは、舵取り及び速度を制御するためにビークルに与えられる命令である。これらの命令は、２−５Ｈｚのレートで供給される。
図５を参照する。前述のように本発明の好ましい実施例では、ＶＰＳ及びナビゲータは共にビークル上に配置され、以下に説明するようにべ一スステーション１８８と通信してホスト処理システム１８６から高水準ＧＰＳ位置情報及び指令を受信する。システムはべ一スステーション及びビークル上においてＧＰＳ衛星２００−２０６からのＧＰＳ位置情報を収集するので、共通モード誤差は除去され、位置決め精度を向上させることができる。
【００９３】
本発明の代替実施例では、ＶＰＳ及びナビゲータの部分をベースステーションに配置することができる。
べ一スステーションに配置されているホストは、例えば点Ａから点Ｂへ行くことをナビゲータに通知することができ、使用する固定ルートの集合の中の１つを指示することができる。またホストは、効率の最大化、衝突回避、誤り状態の検出等々を達成するためのビークル及び機器め調整のような他の典型的なタスク指名及びスケジューリングをも処理する。またホストは、人間の管理者のための操作インタフェースを有している。
通信の隘路と、それによりもたらされる性能及び応答性の劣化を回避するためには、ホストをべ一スステーションに配置し、ナビゲータをビークルに配置することが望ましいことが分かった。ホストは比較的高水準の命令及び簡易化されたデータをナビゲータヘ送るから、比較的狭い通信帯域幅で済む。しかしながら、本発明のために広帯域通信が使用可能である場合には、これが要因にはならないかも知れない。
【００９４】
本発明のシステムの諸要素の特定位置を決定する別の要因は、自律航法の時間臨界度である。あるルートを辿る際に容認できない程の不正確さを回避するために、航法システムはその絶対位置と相対位置とを絶えず検査していなければならない。位置検査に必要な周波数はビークルの速度と共に増加し、ビークル速度が比較的中庸であってさえも通信速度が制限要因になり始める。
しかし、ビークルの最大速度が主たる考察要因ではなく、高度のルート追随精度が臨界的ではない両方または何れか一方の応用においては、この通信要因は重要ではないかも知れない。例えば、広々と開けた平坦な土地を比較的直線の経路に沿って急いで横切るような場合には、曲がりくねった山道に沿って走行する場合のように走行中に屡々位置を検査する必要はあるまい。
【００９５】
概念的には、本発明の航法の面は、任意に以下の主要機能に分割することができる。
ルート計画／経路生成；
経路追尾；及び
障害物処理。
本発明の機能を以下に説明する。
【００９６】
Ｂ．ルート計画／経路生成
１．序
本発明による自律ビークル航法は、概念的には、別個に解決される２つの副問題、即ち経路生成と、経路追尾とからなる。
経路生成は高水準プランナからの中間目標を使用して、ビークル１０２が辿る詳しい経路を生成する。これらの計画の表現の簡易さと、それらを実行できる容易さとの間には明白なトレードオフが存在する。例えば簡単な計画は、経路を直線と円曲線に分解することである、しかしながら、セグメントの遷移点における曲率が不連続であり瞬時加速を必要とするために、簡単にこれらの経路を精密に追尾することはできない。
経路生成に続いて、経路追尾は入力として生成された詳細な経路を入手し、可能な限り精密にその経路を迫るようにビークル１０２を制御する。所要の舵取り運動を正確に達成することに失敗すると、安定なオフセット誤差を生じてしまうので、単に事前に作成された舵取り命令のリストに従うだけでは不十分である。誤差は長期運転中に累積する。グローバル位置フィードバック４３２は、理想的アクチュエータに対する不足分を補償するために使用することができる。従来のビークル制御計画とは異なり、位置の時間履歴（軌跡）をビークル１０２に対して指定された計画の中に暗示するような方法が本発明のために開発された。
これらの方法は、舵取り運動が時間から切り離されていること、即ち舵取り運動を指定された経路の幾何学的性質に直接関係付け、ビークル１０２の速度を独立パラメタとしていることから、“経路”追尾と適切に命名されている。
【００９７】
さて、図３を参照する。自律ビークル１０２は、ホスト処理システム１８６の指令の下に、ダンプ現場３２２まで輸送セグメント３２０を走行し、その積荷をダンプした後に、別の輸送セグメントをサービスショップ３２４まで走行することを要求されるかも知れない。ホスト処理システム１８６はビークル１０２の行先を決定するが、これは“サイクル計画”と呼ばれる。どのルートを通って所望の行先へ行くかの決定は、“ルート計画”によって達成されなければならない。
“ルート計画”とは、どの経路セグメントを通って所望の行先へ行くかを決定することである。一般に、ルートは２つの限定された位置間の点の集合の高水準抽象、または表現と考えることができる。誰かが運転手に対して“メイン州ロブスターから国道９５号線を南にとりフロリダ州マイアミまで行け”と指令したものとすれば、運転手がその指令を一連の操作（これらの操作は、車即ちビークル１０２を始動させ、ブレーキ４４０６をゆるめ、伝導装置４６１０を係合させ、公示された制限速度まで加速し、舵取りハンドル４９１０を回し、障害物を避ける等々を含むであろう）に変換するであろうが、それと全く同じことを本発明の自律航法システムは遂行するのである。本発明のシステムにおいて使用する“ルート”とは、走行の始まりと終りとの間の連続“セグメントのシーケンスのことである。
自律ビークル１０２はシーケンスのどの位置からでも始めることができ、またルートを何れの方向にも走行することができる。“セグメント”は、“ノード”間の経路である、”ノード”はある経路上で決定を必要とする“ポスチュァ”（ｐｏｓｔｕｒｅ）である。ノードの例は、積荷現場３３１８、ダンプ現場３２２、及び戸交差点３２６である。
【００９８】
セグメントには種々の型がある。例えば、線形セグメントと円形セグメントとがある。線形セグメント（線）は、２つのノードによって限定される。円形セグメント（弧）は、３つのノードによって限定される。
“ポスチュア”は、例えばルート、経路、及びノードの部分をモデル化するために使用される。ポスチュアは、位置、ヘッディング（進行方位）、曲率、最大速度、及び経路上の所与の点に関する情報からなることができる。
“経路”は、連続するポスチュアのシーケンスである。
従ってあるセグメントはノード間の連続するポスチュアのシーケンスである。全てのセグメントはそれらに関連するある速度を有しており、この速度はビークル１０２がそのセグメントを走行する時の最大遠度を指定する。他の要求を満たすために、もし必要ならば、ナビゲータ４０６はより遅い速度を命令することができる。
【００９９】
経路セグメントを限定するにはどのポスチュアが必要であるかを解析的、実験的、または両者の組合せによって決定することを、本発明では“経路計画”と呼ぶ。説明の全範囲にわたって上述の連続するルートのシーケンスを“サイクル”と呼び、ビークル１０２の作業目標はその“サイクル”を決定する。
従ってルートを限定するためには、先ずノードとセグメントとを限定しなければならない。次にノードとセグメントとを順序付けなければならない。最後に、ルートの順序付けられた集合の何処から始めるのか、及び順序付けられた集合をどの方向に進むのかを指定することによって、ルートを限定しなければならない（本発明のこれらの概念を示す図２２を参照されたい）。
ルートを限定する上述の方法は、本発明ではメモリ効率のために開発された。またこれは、ノード及びセグメントの指定された集合で多くのルートを限定するための便利な方策でもある。
【０１００】
本発明の実際の例では、多くの交差する道路３２６が存在するような現場が想像される。ルートプログラマは、交差点にノードを限定し、交差点間の道路を限定するためにセグメントを限定するであろう。従って、ルートは道路及び交差点によって決定されるであろう。しかしながら、交差点及び道路の固定された集合を用いて、点Ａから点Ｂまで行くには多くの道（多くのルート）が存在しよう。
本発明の経路追尾方法（以下に説明する）はビークルを舵取りするためにルート曲率を使用する。線と弧を使用するルート限定方法は、連続する曲率を与えない。クロソイド（ｃ１ｏｔｈｏｉｄ）曲線はルートを限定するための別の方法である。
本発明者によって開発された別のルート限定方法は、駆動されたデータにＢスプラインを適合させる。Ｂスプラインは連続的な曲率を与え、従って追尾性能を向上させる。更に、Ｂスプラインは自由形状の湾曲であるから、ルートは単一のＢスプライン曲線によって限定することができる。自由形状の湾曲を使用することによって、ルートを、ルート上でビークルを駆動することによって収集されるデータに適合させるためのより確固たる方法（半自動）が本発明によって得られる。
【０１０１】
図４及び２２を参照する。動作中に、べ一スステーション１８８のホスト処理システム１８６は、識別されたビークル１０２に対してその現位置からルートＮを取るように命令する、ナビゲータ４０６は“ルート１”を、各々が“公示された”または関係付けられた最大速度限界を有することができる一連のセグメントに翻訳することによらて経路を生成する（これらのセグメントが一緒になってビークルに辿らせるための生成された経路を形成する）ように機能する。ルートを指定し、この方向の高水準命令を有する自律ビークル１０２に命令することによって、本発明では方向を与える場合に要求される莫大なデータ及び非効率を回避している。
ナビゲータ４０６は、ルートを一連の個々の点の集合としてではなく、経路セグメントのリンクされたリストとして記憶する。これらのセグメントは、限定された位置即ちノード間の点の集合の抽象でもある。
次いでリンカは柔軟性及び効率を斟酌しながら所与の経路セグメントを取り、制御点のリンクされたリストを生成する。図２２に示すように、経路セグメントは異なるルートによって共用される。
経路セグメントは、弧、線及びポスチュアの集合として、ＴＡＲＧＡ  ５３０２と呼ぶメモリ内に記憶される。例えば、本発明の１実施例では、解析的生成プログラム機能がこれらの弧、線及びポスチュアを使用して経路を生成する。本発明の別の実施例では、上述のように、ルートの数学的表現としてＢスプラインを使用する。本発明の別の実施例では、経路セグメントを生成するために“クロソイド”曲線を使用する。これらに関して以下に説明する。
【０１０２】
ａ．クロソイド経路セグメント
上述したように、本発明が関与し解決しようとする航法問題の部分は、実際には２つの副問題、即ち経路計画及び経路生成である。本発明はこれらを別々に解決する。
経路計画はある経路最適化機能を使用して副目標の集合から開始され、ビークル１０２が取得しなければならない“目的”点の順序付けられたシーケンスを生成する。
経路生成の難題は（経路計画の）目的点から、自律ビークル１０２が容易に辿るのに十分に滑らかな連続した、衝琴を生じない経路３３１２を生成することである。例えば、簡単な計画は経路３３１２を直線と円形曲線とに分解することである。次いで経路３３１２はビークル１０２を所望の経路上に保つためにビークルのアクチュエータに供給される明示指令のシーケンスに変換される。これらの計画の表現の簡易さと、それらが実行できる容易さとの間には明確なトレードオフが存在することに注目すべきである。
自律ビークル１０２が指定された経路３３１２を追尾する能力は、経路の特性に依存する。生成される経路３３１２の曲率が連続していること、及び曲率の変化率（鋭さ）は、これらのパラメタがビークル１０２を所望の経路３３１２上に留まらせるために必要な舵取り運動を指令するものであるから特に重要である。曲率が不連続であると、それらは無限の加速を要求するので追随することは不可能になる。経路の線形の鋭さはほぼ一定速度の舵取りに等しいから、若干の自律ビークル形態の場合には、経路の鋭さが線形である範囲が舵取り運動によってビークルを所望の経路３３１２上に留める見込みがある範囲である。
【０１０３】
本発明が使用する１方法は、直線及び円弧のシーケンスとして経路を組立てることである。この方法は、弧が接し合う所で曲率に不連続をもたらす欠点がある。本発明の別の方法は、目的点間に経路を当てはめる多項式スプラインを使用することである。スプラインは曲率の連続性は保証するが、鋭さの線形性は保証しない。
必須曲率を追尾することができなければ、所望の経路３３１２から安定したオフセット誤差がもたらされる。これらの誤差は、位置３３１４上のフィードバックループを閉じることによって補償することができる。これは、アクチュエータの応答が十分に高速であって追尾誤差が無視でき、また工場の床のように位置検知が正確であることを保証するようなシナリオにおいては十分である。しかし、もし経路が本質的に追尾が容易であれば、経路追尾はより簡単である。
本発明の方法は、目的点のシーケンスを通過している明示経路を生成する。本発明の導関数法は、追尾誤差が大きくなるか、または所望経路が変化する場合には経路の部分を動的に再計画する。
【０１０４】
ｂ．ビークル経路モデル化
どのような経路も、位置座標（ｘ（ｓ）、ｙ（ｓ））３３０４によって経路長（ｓ）の関数としてパラメタ化することができる。即ち、位置座標Ｘ及びｙは経路長Ｓの明示関数として書くことができる、ヘッディング（ｏ（ｓ））３３１８及び曲率（（ｃ（ｓ））３３１６は以下のように導出することができる。
0003560959.tif 000006
これらのパラメタの４部分、ｐ＝（ｘ，ｙ，ｏ，ｃ）は任意点における自律ビークル１０２の状態を時刻で記述するポスチュア３３１４である。
【０１０５】
Ｃ．クロソイド曲線
本発明の１実施例にはクロソイド曲線が使用されている。これらはポスチュア連続である曲線の群であり、それらの曲率が曲線の長さと共に線形に変化することが特徴である。
0003560959.tif 000007
ここにｋは曲線の曲率の変化率（鋭さ）であり、下添え字ｉは初期状態を表す。クロソイド曲線セグメント２００２を図２６に示す。
初期ポスチュア、クロソイドセグメントの鋭さ及びそのセグメントに沿う距離を与えれば、任意点における位置、配向、及び曲率は以下のように計算することができる。
【０１０６】
0003560959.tif 000008
【０１０７】
ｄ．ポスチュア連続経路の生成
実際の航行における諸問題は、単一のクロソイドセグメントでは満足させることができないような距離及び複雑さを有する複合経路を必要とすることである。殆どの経路は、複数の目的点を通過する多重セグメントを必要とする。
（１）従来の方法
Ｐｒｏｃｅｅｄｉｎｇｓ  ＩＥＣＯＮ．１９８５，ＭＩＴ  Ｐｒｅｓｓ，１９８５に所載のホンゴウらの論文”自己制御ビークルの自動案内システム・命令システム及び制御アルゴリズム”は、目的点のシーケンスから接続された直線及び円弧からなる連続経路を生成する方法を提唱している。弧及び直線のみからなる経路は計算は容易であるが、上述したようにこのよ一うな計画はセグメントの遷移において不連続を残す。
Ｒｂｏｂｏｔｉｃｓ  Ｒｅｓｅａｒｃｈ：  Ｔｈｅ  ｔｈｉｒｄ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｓｙｍｐｏｓｉｕｍ，  ＩＳＩＲ，  Ｇｏｕｖｉｅｕｘ，  Ｆｒａｎｃｅ，  １９８６に所載のカナヤマらの論文“移動ロボットのための軌跡生成”は、ポスチュア間にクロソイド曲線と直線の遷移とを対にして使用している。直線遷移の制約は、閉じた形状の解を有していない式（７）及び（８）内の積分によるものである。カナヤマはこの問題を、ｃｉ＝０を要求することによって簡易化している。また参照フレームを初期配向の量、ｏｉ＝Ｏだけ回転させることによって、簡単な近似
【０１０８】
0003560959.tif 000009
【０１０９】
が残る。
カナヤマの方法は、若干の点においてより鋭く、必要以上にコンパクトな経路をもたらし、制御に不都合な結果となる。更に、再計画されるセグメントが非曲線区分を含むことが保証されていないので、直線遷移に対しては経路の局部的再計画を予め除外することが要求される。
【０１１０】
(2)点のシーケンスからの経路生成
点のシーケンスから独特なポスチュア連続経路を生成するための本発明の2段階方法を以下に説明する。
図23、24、及び25を参照する。第1段階は、目的点から独特なポスチュア2302、2304、2306、2308、2310のシーケンスを導出することである。第2段階は、これらのポスチュアをクロソイドセグメントで補間することである。開始位置2402及び終り位置2404においてヘッディング及び曲率を仮定する。開始ポスチュア2402及び終りポスチュア2404を、それぞれＰi及びＰfとする。
4つの式、式2、式4、式5及び式6を1つのクロソイド曲線の2つのパラメタ(鋭さk及び長さs)だけで同時に満足させることはできないから、2つのポスチュアを1つのクロソイド曲線セグメントで接続できるとは限らない。
4つの式、式2、式4、式5及び式6を満足させるためには、少なくとも2つのクロソイド曲線セグメントを必要とする。しかし、もしｋi及びｋrが同一符号を有していれば、2つのクロソイドセグメントでは一般的な問題は解くことはできず、殆どの場合それらの間に第3のセグメントが必要になる。1対の隣接する関連ポスチュアを接続する一組の十分なクロソイドは、3つのクロソイドセグメント(k,si)、(一k,s。)、(k,s3)の集合である。下添え字は、Ｐiからのクロソイドセグメントの順位である。この組合せは、以下の理由から妥当であると考えられる。
【０１１１】
１．最初と最後のクロソイドセグメントのためのｋの符号が同一であること。
２．第２のクロソイドセグメントのためのｋの大きさが等しく、符号が最初と最後のクロソイドセグメントの符号とは反対であること。これにより、最初と最後のクロソイドセグメントの符号が曲線位置要求を満足しているとしても、Ｓ１、Ｓ２、Ｓ３を変化させることによって開始及び終り曲率間の曲率変化を３つのクロソイドセグメントの曲線で満足させることができる。
３．組合せ内には４つの変数、ｋ、Ｓ１、Ｓ２、Ｓ３が存在すること。開始及び終りポスチュア間の数学的関係を記述する以下の４つの式を満足させる独特な解を見出すことが可能である。
【０１１２】
0003560959.tif 000010
0003560959.tif 000011
【０１１３】
ここに、
θ１（ξ）＝θｉ＋Ｃｉξ十（ｋ／２）ξ１２、
θ２（ξ）＝θｉ＋ＣｉＳ１＋（ｋ／２）Ｓ１２＋（ｃｉ＋ｋｓ１）ξ一（ｋ／２）ξ２、
θ３（ξ）＝θｉ＋Ｃｉ（ＣｉＳ１）十ｋ（Ｓ１Ｓ２）十ｋ／２（Ｓ１２−Ｓ２２）
＋｛Ｃｉ＋ｋ（Ｓ１−Ｓ２）〕ξ十（ｋ／２）ξ２
である。
【０１１４】
図２７に示す方法を参照する。上記式９及び１０は閉じた形状の解が存在しないフレネル積分を含むから、ｋ、Ｓ１、Ｓ２及びＳ３の値が計算される。
この方法から得られる経路は、他の方法に比して以下のような長所を有している。
§この方法は、点の任意のシーケンスから進められる。普通は目標を点を発展させた列として配置してある探査計画ではポスチュアの生成は不可欠である。この方法によって生成された経路が全ての目的点を通過するのに対して、カナヤマの方法及び弧法は、これらの方法がポスチュアのシーケンスから出発しているために、多くの点に近接しているに過ぎない。
§この方法は、経路に沿う位置、ヘッディング及び曲率の連続性を保証する。更に、鋭さは区分的に一定である。
§この方法によって生成される経路は、道筋の諸点を直線で接続することによって形成される鋭角の外側を常に通る。得られた経路は、一般にこれらの角度の内側に存在する障害物の周囲を補間するのに特に有用である。これに対して、カナヤマの経路は常に角の内側にある。
【０１１５】
(3)経路のクロソイド再計画
クロソイド再計画は、始めに経路を取得するか、または本発明による正常航行を通してビークル102を所望経路3312へ戻るように案内するかの何れかのために行われる。
予め指定された経路を追尾する際に、大きい修正を行おうとして突然に加速するのを回避するために、本発明は現位置から所望経路3312へ滑らかに収束する新経路を生成する経路プランナを使用する。再計画は2つの副問題に分解される。
【０１１６】
１．企図した経路３３０８への収束点の決定。
２．現位置３３０２から収束点３３０８までの経路の計画。
本発明による経路再計画を図式的に示す図２８を参照する。予め指定された経路は、ポスチュア（ｋ，ｓ）ｍ（ｍ＝１，．．．，ｎ）２８０４−２８１０と、ポスチュアＰｍ（セグメント（ｋ，ｓ）ｍの終りに位置する）との間の補間２８０４からなる。ビークル１０２が、ＰｍとＰｍ＋１との間の経路からずれているものとすれば、再計画された経路２８１６が収束するポスチュア３３４としてＰｍ＋２が選択される。Ｐｍ＋２までの距離は可変である。
２つの曲線からなる曲線がポスチュア（現ポスチュア及び収束ポスチュアとして選択されたポスチュア）に適合され、４つの支配的ポスチュア式、式７、式８、式９、式１０を満足する再計画された経路２８１６が得られる。もし、経路を再計画するのか否かを決定するしきい値が各クロソイド曲線セグメント（ｋ，ｓ）ｍの長さよりも遥かに小さければ、既知（（ｋｍ＋１，Ｓｋ＋１）、（ｋｋ＋１、ｓｋ＋１））からの小さい動揺を使用して新しいポスチュア連続経路（（ｋ＊ｋ＋１，Ｓｋ＋１）、（ｋ＊ｋ＋２，Ｓ＊ｋ＋２））を見出すことができる。再計画された経路２８１６は元の経路３３１２から極めて遠いとは考えられないから、２つのクロソイドセグメントを使用することができる。
【０１１７】
(4)概要
本発明による自律ビークル102のための連続経路の生成は、得られる経路がポスチュア連続であるだけではなく、曲線に沿う線形曲率が経路に沿ってほぼ線形に変化する舵取り角をもたらし、経路追尾を容易ならしめるので、経路生成にクロソイドセグメントを使用することができるのである。
本発明の方策は以下のようである。先ず、目的点を使用してポスチュアのシーケンスを求める。次いで、各隣接ポスチュアを3つのクロソイド曲線セグメントを使用して接続する。
本方法は、弧及びO曲率クロソイドを使用する場合のように目的点の事前処理を必要としないという付加的な長所が得られる。更に、生成された経路のジオメトリは、道筋の諸点を直線で接続することによって形成される鋭角の外側を常に通る。これらは、普通は角の内側に存在する障害物の周囲を補間するのに特に有用である。
記憶した弧、線及びポスチュアの集合、クロソイド曲線、Bスプライン等から、VPSポスチュアブロックを用いてある経路に沿う諸点が生成される。
以上のような本発明のルート処理の長所は、ホストとビークルとの間に要求される帯域幅が減少することに加えて、記憶するデータを減少させるために要求されるデータ圧縮が行われること、及び経路を滑らかにするように機能することである。
【０１１８】
(5)Bスプライン
Bスプラインは、多項式の係数を指定することによって一連の点の形状を記述する手段として数学的に、及びコンピュータグラフィックスの分野では周知である(ニューヨーク州ニューヨークのマグロウヒル・ブック・カンパニから刊行されているDavid F. Rogers及びJ. A1ah Adams著“コンピュータグラフィックスのための数学的要素"144-155頁を参照されたい)。この曲線近似関数はN次の多項式である(Nはユーザが指定し、曲線の所望の形状に依存する)。Bスプライン曲線は如何なる次数であってもよく、曲線近似関数マイナス1の次数に連続する。
Bスプラインは本発明の実施例に使用されている。Bスプラインは、少数の係数によって任意長の経路を記述でき、記憶するデータ量を減少させるので、本発明における経路生成には十分に役立つのである。曲線近似関数の次数が十分に高い(3またはそれよりも大きい)ものとすれば生成される経路は曲率が滑らかであり、本発明の上述の実施例が本質的に容易に追尾できる経路が得られる。
図29にBスプライン曲線の例を示す。
【０１１９】
２．ルート作成及び記憶
ａ．序
本発明の１実施例においては、現場３００のためのルートを作成するために、人がビークル１０２を作業現場３００の道路系上を駆動している間に、先ずＶＰＳ１Ｏ００からデータを収集して記憶する。次いでノード及びセグメントを記憶された駆動されたデータに当てはめ、上述の手順毎のルートに編成する。
ルートデータを記憶された駆動されたデータに図式的に当てはめ、更にルート（即ち、速度、シーケンス、出発点、走行方向）を限定するために、ＡＰＯＬＬＯコンピュータ（現カリフォルニア州パロアルトのＨＥＷＬＥＴＴ−ＰＡＣＫＡＲＤ）ワークステーション上での応用が開発された。このＡＰＯＬＬＯと同等などのようなグラフィックスワークステーションも使用可能である。
現場のためのルートが限定されると、そのルートデータは永久記憶装置へ書き込まれる。本発明の１実施例で使用した記憶装置は、付属読み取り装置／書き込み装置を有するバブルメモリカートリッジである。バブルメモリ装置５３０２は恒久性であり、電力が切り離された時にデータを保持する。ＡＰＯＬＬＯ応用はカートリッジ５３０２ヘデータを書き込むことができ、カートリッジ５３０２からデータを読み取ることができる。
先に暗に示したように、本発明においてはルートは予め限定することも、または動的に生成することも可能である。
採鉱応用では、一般的に、現場３００が探査され、道路は予め計画され、注意深く設計され、そして建設される。次いで航法システムによって使用されるルートが、手動で作成された（その航法システムによって使用されるように特別に作成された）コンピュータデータベースから、または代替として、上述のようにビークルを現場の実際のルート上で物理的に駆動してルートを学習させるの何れかによって入手される。学習方法では、所与のルートを数回走行させることができる。次いでデータの変動（例えば運転手のハンドルの切り方による）が平均化され、滑らかな最良適合が開発される。
【０１２０】
ｂ．ルート限定
本発明の１実施例では、ルートを限定するために以下の方法を使用している。ルートを構築するノード及びセグメントを限定。ノード及びセグメントデータを“ルートデータ”アレイと呼ぶアレイ内に配置する。アレイ内の各記録は以下の情報を含む。
１．項目の型（即ち、ノード、線形セグメント、円形セグメント、ルートマーカの終り）
２．もしノード項目であれば、そのノードの北及び東座標を限定
そうではなく、もし線形セグメントであれば、そのセグメントに沿う速度を限定そうではなく、もし円形セグメント項目であれば、中心の北及び東座標、半径、円が走る方向（即ち、時計方向または反時計方向）、及びそのセグメントに沿う速度を限定
そうではなく、もしルートマーカであれば、他の情報は存在しない。
【０１２１】
２．ノード及びセグメントデータを一緒にしてシーケンスにリンクする。これらのシーケンスは、単なるルートデータアレイ内への索引のアレイである。各シーケンスはルートマーカの終りで始まり、ノードが後続し、次いでシーケンスの残余は、そのシーケンスが別のルートマーカの終りによって終了させられるまで、セグメントとノードとの間を交番しなければならない。シーケンスの例は以下のようであろう。
１，６，３，４，７，９，１Ｏ，２３，７８，１
但し、これらの整数はルートデータヘの索引である。
３．最後に、シーケンスアレイヘの索引と、シーケンスを通して正方向に索引付けるか負正方向に索引付けるかを指定することによってルートを限定。索引及び索引方向を“ルートスペック”アレイと呼ぶアレイ内に配置する。ルートスペックアレイ内の項目は以下のようであり得る。
６，１          この仕様は、ノード６で始まり、正方向に索引付けられたルートを限定する。
７８、一１この仕様は、ノード７８で始まり、負方向に索引付けられたル
ートを限定する。
ユーザは、単にビークルに、ルートスペックアレイ内のどの項目をルートとして使用するかを通知するだけである。
４．上記データを、段階１−３において限定した順番に記憶装置内に記憶。
【０１２２】
Ｃ．ナビゲータルート用法
以下の説明は、本発明の上記方法から限定されたルートをナビゲータ４０６がどのように使用するかに関する。
ナビゲータ４０６は給電されると、ルート情報を記憶装置５３０２から読み取り、それを既存の構文内のＲＡＭに記憶する。
次に、操作員はビークル１０２が辿るルートを指定する。この場合も、ルートはルートスペックアレイ内への索引である。
ナビゲータ４０６は、全てのシステムが自動動作の準備が整ったことを決定すると、メッセージをｖｐｓポスチュアタスク５３２４へ送って係合することを通知する。    ｖｐｓポスチュアタスク５３２４は、ビークル１０２の現位置２８１２に最も近いルートに沿う位置を決定する。ルートに最も近い位置２８４の探索は以下のように進められる。
１．ポインタを、そのルート内の最初のセグメントにセットする。
２．ビークル位置からそのセグメントまでの垂直距離を決定する。
３．ポインタを、そのルート内の次のセグメントに移動させる。
４．ビークル位置から次のセグメントまでの垂直距離を決定する。
５．ルートマーカ２２１８の終りに達するまで段階３及び４を繰り返す。
６．ビークル位置からそのルートの端点２２１８までの距離を決定する。
７．ポインタを、最も近い距離であったルートセグメントにセットし、その最も近い距離の座標を記憶する。次いでｖｐｓポスチュアタスク５３２４は、ルートのこの記述（線、弧及び速度）を使用して、１メートル間隔でポスチュアを生成する、タスク５３２４は、ポスチュアの予め限定された距離プラス安全余裕度を生成し、ポスチュアをバッファ３０００内に配置する。所与のポスチュアから１メートルであるポスチュアを生成するために、ｖｐｓポスチュアタスク５３２４は以下の手順を使用する。
【０１２３】
１．所与のポスチュァを生成したセグメントの型を決定する。
２．セグメントの型に関する適切な公式を使用してセグメント長のメートル当たりの北及び東の変化を決定する。
３．メートル当たりの北及び東の変化を最後の所与のポスチュアに付加する。
４．もし生成したポスチュアが現セグメントの端から離れていれば、ポインタを次のセグメントにセットし、段階２及び３を繰り返す。そうでなければ、生成したポスチュアを戻す。
次いでｖｐｓポスチュアタスク５３２４は、追尾の準備が整ったことをエグゼクティブ５３１６へ通知する。
【０１２４】
自律ビークル１０２がバッファ３０００内のポスチュアに沿って移動するにつれて、安全余裕度３００６が枯渇してくる、安全余裕度が指定された量以下になると、ｖｐｓポスチュアタスク５３２４はポスチュアの別の安全余裕度３００６を生成し、それらを現バッファ３０００へ追加する、ｖｐｓポスチュアタスク５３２４は、ビークル１０２の現位置２８１２を監視しバッファ３０００内のポインタ３００２を最寄りのポスチュアヘ移動させることによってポスチュアバッファ３０００を枯渇させる。ポスチュアバッファ３０００は時計方向に進行するリングとして構成されている（図３０のポスチュア・リングバッファ参照）、即ち、ポスチュアは、ビークルの走行方向がポスチュア・リングバッファ３０００の時計方向進行に対応するようにリング内に配置されるのである。従って、ビークル１０２が運動するにつれて、バッファ３０００内の最寄りのポスチュアを指すポインタ３００２は時計方向に移動して行く。ポインタ３００２が時計方向に移動すると、ポスチュアの背後（ポインタの反時計方向）のリング内のメモリは自由に重ね書きされるようになる。
【０１２５】
段階７（上述の探索ルーチンの）はルートマーカ２２１８の終りがリセットされるまで登録され、その時点になるとｖｐｓポスチュアタスク５３２４はポスチュアの生成を終らせ、ルートの終りに達したことをエグゼクティブ５３１６へ通知する。
前述のように、経路は連続“ポスチュア”のシリーズ、またはシーケンスである。ポスチュアはトラック上に載っているのに必要な速度と舵取り角を含む。ポスチュアは、経度、緯度、ヘッディング、曲率（１／転回半径）、最大速度、及び次のポスチュア情報までの距離を含むことができる。
【０１２６】
３．ポスチュア生成
本発明の追尾方法は、追尾するルートに関するある情報を必要とする。この情報は、”ポスチュア”３３１４と呼ばれるパケット内に含まれている。単一のポスチュア３３１４は、ルート上の指定された位置（ｌｏｃａｔｉｏｎ）に関する位置（即ち、北及び東座標）、ヘッディング、及び曲率を含むことができる。従って、本発明によればルート仕様からポスチュアデータを発生する方策が必要である。
ナビゲータタスク（以下に説明）の中には、ルート情報を読み取り、追尾方法が使用するルートに沿う間隔（例えば１メートル）でポスチュアを発生するタスクがある。本発明の１実施例では、各ポスチュアはメモリの３６バイトを必要とし、これは、ルートの１キロメートル毎に約３６ｋになる。メモリに対する要求を低減させるために、ナビゲータはポスチュアデータを緩衝する。
ポスチュアを発生するタスクは、ビークル１０２の現位置を読み取り、ルート上で現位置に最も近い点を見出し、次いでビークル１０２の前方に指定された数のポスチュアを生成する。生成するポスチュアの数は、ビークル１０２の最大停止距離に依存する。即ちバッファ３０００内には、ビークル１０２を停止点まで案内するために十分なポスチュアが常に存在すべきなのである。
しかし本発明によるルート限定のためのＢスプライン方法では、追尾方法がＢスプライン曲線からポスチュア情報を直接発生することができるので、ポスチュアバッファは必要としない。
【０１２７】
Ｃ．経路追尾
１．序
経路追尾または追随は、本発明によるビークル航行の重要な面である。本発明の技術は、正確な自律ビークル経路３３１２を追随させるために位置をべ一スとする航行（普通の航行システムに使用されている視覚をべ一スとする航行ではない）を使用する。また本発明は、舵取り角３１１６とビークル速度３１１８とを別々に制御することが新機軸である、図３６に本発明の経路追尾システム３６００を図式的に示す。
指定された経路を本発明による自律ビークル１０２に追尾させるためには、ビークルサーボ制御装置のための参照された入力を生成する必要がある、即ち、経路追尾は、現在のずれた位置から前方の参照された経路へ戻るために、次の時間間隔のための参照された舵取り角と参照速度とを求める問題と考えることができる。
一般的に言えば、経路追尾は、所与の経路を辿るのに必要な自律ビークル命令（速度、舵取り角）を決定することである、予め指定された舵取り角、駆動輪速度値及び誤差成分が与えられれば、命令舵取り及び駆動入力は本発明により計算される。
【０１２８】
２．検討
ａ．グローバル位置フィードバック
追尾すべき経路は直交座標で指定される。もし制御計画が参照舵取り命令に対するサーボ制御だけからなっていれば、ビークル位置及びヘッディング誤差は累積する。位置及びヘッディングは舵取り及び駆動の全履歴を積分することによって得られる。従って、ビークル位置３３０４及びヘッディング３３１８は直交空間でフィードバックする必要がある。従って、サーボ制御装置への参照された入力は、位置付けされたフィードバック３１１４（図３１に示すように）に基づいて実時間で生成される。
ｂ．分離舵取り及び駆動制御
舵取り及び駆動参照入力は、本発明ではそれぞれ所与の経路及びビークル速度から計算される。これは経路追尾と、例えば衝突回避のような本発明の他のモジュールとの統合を容易ならしめる。
【０１２９】
３．実施例
ａ．追尾制御構造
（図３１及び図３３）
ビークル自律の難題の１つは、指定された経路を追尾するために要求される舵取り入力を決定することである。普通に舵取りされるビークルの場合、本発明では所望の経路及びその経路に沿う所望の速度は別々に追尾することができ、舵取りの制御の１つに対する問題が減少する。（この説明では、位置の時間履歴である軌跡に対して、経路とは時間には無関係な幾何学的曲線のことを言う。）
舵取り角は所望の経路３３１２及び検知されたビークル位置から計画される。これらの角は舵取り制御装置３１０４を介してビークルに命令される。
図３１の機能ブロック線図は、本発明による追尾制御構造を示す。
運動学的舵取り計画では、位置、ヘッディング及び曲率の誤差は、アクチュエータ飽和、コンプライアンス、何等かの摩擦または質量項を考慮することなく、誤差のジオメトリに基づいて低減される。良好な性能を得るために、経路上の予見距離及び曲率の選択のような調整を要する値が経験的な試行及びシミュレーションを通して選択される。
【０１３０】
手動駆動ビークルでは、予見距離は、駆動中に運転手が見るビークルの前方の距離３３１０である、本発明における予見距離は、それまでに位置、ヘッディング及び曲率の誤差をＯに低下させようとする距離である。この距離は普通の、または自律ビークルの速度と共に変化する。
予見距離を変化させると、コースの変化を遂行するために舵取り調整を行わなければならない程度が変化する。予見距離に関しては後に詳述する。
しかし、実際のビークルは運動学的に理想化されたものとは異なり、それらの制御応答も相応に異なる。ビークルの速度、質量及び経路状態が変化すると、実際のビークル応答は運動学的理想からより一層異なってくる。従って、一般的に運動学的理想は一定の状態を有する低速度においてのみ有効である。
【０１３１】
本発明の実施例は、こわさ、質量及び滑り角に関する考えを含むモデルを使用する。制御問題は線形二次最適追尾問題として定型化され、位置、ヘッディング及び曲率の誤差はビークル制御モデルに基づいて最小化される。
最適経路及び制御は、現誤差を最適制御問題への初期条件として使用して所望の経路及び現在検知されているビークル位置から計算される。最適経路の初期部分に沿う幾つかの計算された舵取り角が、次の検知時間間隔のための低レベル舵取り制御装置への参照として使用される。
この予告最適舵取り計画は、所与の性能指標に対して安定性と最適性とを保証する長所を有している。本発明のこの最適予告制御方法が、自律ビークルの舵取り計画の中枢である。
【０１３２】
再度図３１を参照する。舵取り制御３１０４の内側ループ３１１６は１０ミリ秒程度で実行され、一方外側ルーフ３１１４はＯ．２５−Ｏ．５秒のレートで閉じられる。
ループを位置で閉じるために以下の手順が使用される。現位置（ＰＡ，ｋ）３２１０を検知すると、現時間間隔の終り（Ｐａ、ｋ＋１）３２１６におけるポスチュアが予測される。  次いで次の時間間隔の終り（Ｐｄ、ｋ＋２）３２１８における所望のポスチュアが計算され、（Ｐａ、ｋ、ｋ＋１）３２１６と（Ｐｄ、ｋ＋２）３２１８との間の参照された舵取り角が決定される。
重要なのは、上述のように、本発明のこれらのビークル及び経路技術が舵取り制御をビークルにおける速度制御から切り離していることである。
【０１３３】
ｂ．５次法
詳細は後述する図５３のナビゲータタスク図には、トラッカ５３０６と呼ぶ機能ブロックが示されている。トラッカ５３０６は所望の、または正確な経路に戻す滑らかな経路を構成するように動作する。本発明の１実施例では、前述のように、５次法（ｑｕｉｎｔｉｃ  ｍｅｔｈｏｄ）が使用される。これは舵取り命令のための誤差空間内に５次曲線を含む。
本発明の５次多項式法は、ある予見距離３３１０内で所望経路に収束する単純な連続経路を再計画し、次の時間間隔に迫るべき再計画された経路２８１６の部分に対応する舵取り角を計算する。
もし所望経路を位置の連続関数として考え、またビークルが現在Ｐａ３３２０にあるものとすれぱ誤差ベクトルを計算することができる（図３３）。この誤差ベクトルは、Ｐ。に対する経路までの横方向距離の誤差（ｅＯ）３３２２と、ヘッディングの誤差（Ｂ。）３３２２と、曲率の誤差（ｙ。）３４０４とを表す、もしビークルを距離Ｌ３３１０（参照経路に沿って測定）以内で指定された経路に戻すのであれば、初期誤差及びＰＬにおけるＯ誤差に対応する６つの境界条件を示すことができる。
0003560959.tif 000012
５次多項式は、再計画された経路（誤差空間における）を以下のように記述するように構成することができる。
0003560959.tif 000013
ここに、ｓは〔Ｏ，Ｌ］の集合内にある。
ｅ（ｓ）のための式は、Ｐ０３３０４からＰＬ３３０８までの再計画された経路２８１６に沿う誤差を与える。２次導関数は経路曲率を記述し、曲率自体は所望経路３３１２へ戻すようにビークルを案内する舵取り命令を計算するために使用される。再計画された経路２８１６からの舵取り角（または誤差空間）の変動は誤差関数ｅ（ｓ）の２次導関数から計算される。次いで、新経路に沿う曲率を次のように計算することができる。
0003560959.tif 000014
【０１３４】
新経路に沿う参照舵取り角３１１２は曲率から変換することができる。この手順は計画間隔毎に実行されるから、参照経路３１１２へ戻る新経路は全て不要である。次の時間間隔のための舵取り角だけが新経路上の点の曲率から計算され、これは次の時間間隔中に達成できる。
予見距離Ｌ３３１０は、如何に早くビークルを舵取りして所望経路へ収束させるかを調整するのに使用できるパラメタである。更に、もしＬ３３１０をビークル速度に比例して選択すれば、より良好な性能が得られる。何故ならば、Ｌ３３１０が小さい値の場合にはビークルは経路３３１２を中心として振動し、またＬ３３１０が大きい値の場合には５次多項式によって導入される変化が十分に小さくなって追尾性能を貧弱にするからである。
６つの境界条件、即ちｅｐ０一現位置における位置の誤差（距離）及び（予見）と、ｅｈ０一ヘッディングの誤差及びｅｈ１（予見）と、ｅｃ０一曲率の誤差及びｅｃ１（予見）とを使用しているから、５次曲線が必要なのである。これは舵取り角３１１２を生成するために使用される。
【０１３５】
経路追尾計画は、一般に、指定された経路が本質的に追尾が容易である場合により良好な追尾を遂行することを思い出されたい。これは、舵取りアクチュエータがビークルの速度に比して緩速である場合に特に言えることである。
舵取り応答、舵取りバックラッシュ、ビークル速度、サンプリング、及び計画時間間隔のような他のビークル特性が、ビークル性能に重大な影響を与える。予測されるように、もし検知及び計画時間間隔を一定に保つのであれば、ビークルの速度が大きいほどより高速でより正確なアクチュエータが必要である。
一般的に言えば、５次多項式方法の長所は、それが簡単であり、且つ参照舵取り角を極めて容易に計算できることである。しかしながら、制御計画にビークル特性（質量、慣性、時間遅延、ビークルと大地間の相互作用等々）を考慮に入れていないので、安定度及び収束は保証されない。
パラメタＬ３３１０（予見距離）はビークルの応答を変更するように調整可能であり、またＬ３３１０の値は、試行錯誤に基づいて選択することができる、この計画により、本明細書の作成時には、約２８Ｋｍ／時までの速度において良好な結果を得ている。
【０１３６】
本発明のトラッカによって使用される方法は以下の通りである。
（１）位置の状態を平均化または評価の何れかによって次の位置を推定し、
（２）上記何れかの推定方法を使用して遅延を補償し、
（３）異なる速度において動的予見一５次法の係数；予見距離を変更する。
Ｃ．待ち時間及び低速システム応答
本発明の別の経路追尾実施例は、ビークル応答特性を改善するために種々の補償技術を使用している。これは、改善された追尾性能を実現するために５次多項式方法と共に使用される。
若干のビークル応答特性は、ビークル制御命令の待ち時間と、遅いシステム応答と、ビークル・大地相互作用（ＶＧＩ）（滑り角及びアンダ／オーバステア）を含むビークル動的特性とを含む。
本発明の１実施例においては、時間遅延を減少させるようにビークル制御ハードウェアを変更し、現存する遅延を補償するように制御命令を十分に早めにセットする方法を使用することによって、ビークル制御命令の待ち時間を補償している。
【０１３７】
ビークルの位置を検知する時刻と、命令を発行する時刻との間の時間遅れを短縮させると予測誤差が減少するが、この減少は舵取り角を計画するために要求されることであり、より良好な追尾性能がもたらされる。
速度と共に予見距離を変化させることも、定予見距離に比して追尾性能を改善する。
追尾方法は、直列リンクを通してビークル制御システムヘ舵取り及び速度命令を出力する。ビークル制御システムは、タスク間通信に関してメールボックス待ち行列を頼りにするマルチプロセッサ、多重タスキングシステムである。
このメールボックス待ち行列は２つの型の待ち行列、即ち高性能待ち行列と、オーバフロー待ち行列とからなる。追尾タスクからの高データ流レート中、高性能待ち行列はオーバフロー待ち行列内へこぼれ、タスク間通信の性能を劣化させる。これは追尾タスクと、実際の舵取りアクチュエータ命令との間の合計待ち時間を秒のオーダーにする可能性がある。
舵取りダイナミックスは１次遅れシステムとしてモデル化することができる。これは、所望の最終値の約６３に到達するまでの１次遅れ応答に対する１時定数に等価な期間を要する。大きい時定数を有する低速システムの場合には、応答時間が重大となり得ることは明白であろう。
待ち時間及び応答問題を解消するために、ハードウェアを、ビークルの舵取りを制御するための追尾方法と、純時間遅延及び貧弱な応答を補償するために考案された新しい制御計画とに密接に関係付けて使用するように調整することができる。
【０１３８】
残余の遅延（追尾方法の処理時間及び追尾システム内のタスク間通信に起因する遅延）を補償するために、本発明では何等かの遅延に反作用するように早めに速度及び舵取り命令を送る方法を使用する。この方法は以下のように実行することができる。
現位置Ｐａｃｔｕａｌを検知
（初期化：Ｐａｃｔｕａｌ＝Ｐ［０］＝Ｐ［１］＝．．．．＝Ｐ［ｄ−ｉｎｄｅｘ＋１］）
予測位置と検知位置との間の誤差を計算：
ｉ＝Ｏの場合ｄ＿ｉｎｄｅｘ，Ｐｅ＝Ｐａｃｔｕａｌ−Ｐ［０］、
Ｐ［ｉ］＝Ｐ［ｉ＋１］十Ｐｅ
時間間隔の始めの位置に対応する経路上の位置を計算：Ｐｏｎを入手
（Ｐ［ｄ−ｉｎｄｅｘ］、Ｐｏｎ）
初期条件を入手：誤差（０）＝Ｐ［ｄ−ｉｎｄｅｘ］一Ｐｏｎ
誤差空間［１］における５次多項式曲線を計算
計画時間間隔の終りにおける位置を予測；
ｄｅｓｐｏｓ（Ｐｏｎ、ｄｓ、Ｐ＋ｄ−ｉｎｄｅｘ＋１）
Ｐ［ｄ−ｉｎｄｅｘ＋１］十＝誤差（ｄｓ）を入手
例えば、２計画間隔程度の（２５０ｍＳｅｃのオーダーの）時間遅延を有するシステムを補償するためには、変数ｄ−ｉｎｄｅｘを２．０にセットする。
追尾性能は、補償指標（ｄ−ｉｎｄｅｘ）がシステムに固有の遅延に整合するように増加するにつれて改善される。
【０１３９】
ｄ．ビークル・大地相互作用（ＶＧＩ）：
舵取り角及びビークル速度に関する参照命令は、変化する角速度とビークル車輪の加速をもたらす。
ＶＧＩは、舵取りされた車輪角及び車輪角速度を与えるとビークルがどのように運動するかを記述する。主たるＶＧＩ現象は滑り角及びアンダ／オーバステア特性であって、これらはタイヤ／道路接触領域ジオメトリに基づき、タイヤの弾性変形によって影響を受ける。これらの現象は、運動学的に計算されたものに比してより大きい舵取り角を要求する。
ｅ．検知及び作動タイミング
本発明においては実際の経路追尾はディジタルプロセッサによって制御されるから、離散した時間間隔が使用される。これは舵取り計画に必要な計算時間（１６ｍＳｅｃのオーダーであり得る）よりも遥かに長い位置検知時間間隔（Ｏ．２５Ｓｅｃのオーダーであり得る）によって支配される。
時には、特に離散した時間間隔が長い場合には、ビークル位置の予測が貧弱となって追尾方法の性能が劣化することが起こり得る。
【０１４０】
0003560959.tif 000015
【０１４１】
本発明の補償方法は、離散した時間間隔を短縮することによって次のビークル位置の予測の誤差を減少させるのに役立つ。この方法では、ビークル位置は、計画間隔（２５０ｍＳｅｃ）の終りにではなく、計算間隔（１６ｍＳｅｃ）の終りに対して予測される。この方法は以下のように実行される。
現位置Ｐａｃｔｕａｌを検知
（初期化：Ｐａｃｔｕａｌ＝Ｐ［０］：Ｐ［１］＝．．．．＝Ｐ［ｄ−ｉｎｄｅｘ＋１］）
＝Ｐａｃｔｕａｌ，ｋ＋１
予測位置と検知位置との間の誤差を計算：
ｉ＝０，ｄ＿ｉｎｄｅｘの場合、Ｐｅ＝Ｐａｃｔｕａｌ−、Ｐａｃｔｕａｌ，ｋ＋１
Ｐ［ｉ］＝Ｐ［ｉ＋１］十Ｐｅ
時間間隔の始めの位置に対応する経路上の位置を計算：
Ｐｏｎ（Ｐ［ｄ−ｉｎｄｅｘ］、Ｐｏｎ）を入手
初期条件を入手：誤差（Ｏ）＝Ｐ［ｄ−ｉｎｄｅｘ］一Ｐｏｎ
誤差空間における５次多項式曲線を計算
誤差空間［１］における５次多項式曲線を計算
計画時間間隔の終りにおける位置を予測
ｄｅｓｐｏｓ（Ｐｏｎ、ｄｓ、Ｐ＋ｄ−ｉｎｄｅｘ＋１）
Ｐ［ｄ−ｉｎｄｅｘ＋１〕十＝誤差（ｄｓ）を入手
次の検知時刻における位置を予測：
Ｐａｃｔｕａｌ，ｋ＋１＝Ｐ［０］十（Ｐ［１］一Ｐ［０］）＊（ｄｔ計画一ｄｔ計算／ｄｔ計画）
【０１４２】
ｆ．予見
人の運転手が駆動する場合には、異なる予見距離３３１０を使用する。低速時には運転手はビークルに比較的近い道路上の点を見ているのが普通であり、この点は一般にビークルより前方である。速度が大きくなるにつれて参照点は更に前方になり、その結果舵取り修正はより小さくなる。
従って、自律応用において予見距離を速度と共に変化させることは追尾性能を改善するのを論理的に援助する。
所望の舵取り角は、参照経路３３１２からの舵取り色と、追尾誤差を修正するために５次法を用いて計算された舵取り角３１１２とからなることができる。これらの舵取り角は加算されて以下の式（１）に示すようなビークル舵取り命令が求められる。
【０１４３】
Φ＝Φｒｅｆ＋Φｅｒｒｏｒ
たとえ手動駆動における予見が参照及び誤差補償舵取り角の両方に影響を及ぼすとしても、自律計画における予見はΦｅｒｒｏｒだけにしか影響を与えないことに注目されたい。予見値を短くすれば大きい舵取り修正がもたらされるから、予見距離は誤差フィードバックシステムにおける利得と解釈することができる。
予見距離（Ｌ）を速度（Ｖ）に伴って変化させる任意モデルは以下の式（２）に示すように３つのパラメタ、Ｖｒｅｆ、Ｌｒｅｆ、及び勾配を用いて表される。
Ｌ＝勾配＊（Ｖ−Ｖｒｅｆ）十Ｌｒｅｆ
ここに、Ｖはビークルの速度であり、ＬはＬｍｉｎ＝１Ｏ乃至Ｌｍａｘ＝３０とすべきである。追尾性能は本発明の変化する予見距離３３１０によって改善される。
【０１４４】
ｇ．最適制御法
前述のように、本発明の実施例は、こわさ、質量及び滑り角に関する考えを含むモデルを使用する。
制御問題は、位置、ヘッディング、及び曲率の誤差がビークル制御モデルに基づいて最小化される線形２次最適追尾問題として処理される。最適経路及び制御は、現在の誤差を最適制御問題に対する初期条件として用いて所望経路３３１２及び現検知ビークル位置３３０４から計算される。
最適経路の初期部分に沿う幾つかの計算された舵取り角が、次の検知時間間隔のための低レベル舵取りコントローラヘの参照として使用される。この予測最適舵取り制御は、所与の性能指標に対する安定性及び最適性を保証する長所を有している。本発明による最適予測制御方法は自律ビークル１０２の舵取り計画に適用可能である。
【０１４５】
モデルは標準の時間短縮された、または自転車モデル（図示してない）、またはビークルの近似から導出される。ビークル運動を記述する方程式は、前述したＶＧＩを表す項を含む。これらの方程式は状態変数を使用する。
0003560959.tif 000016
ここにｘ及びｙはビークルのグローバル位置を表し、θはビーグルのヘッディング３３１８であり、θはヘッディングの変化のレートである。
これらの変数を使用する方程式は以下に通りである。
0003560959.tif 000017
0003560959.tif 000018
0003560959.tif 000019
0003560959.tif 000020
0003560959.tif 000021
ｕ１＝δ          Ｃ＝［ｍＬ／ｔ］
但し、
Ｖξ：横方向速度
Ｖｎ：一定前進速度
δ：舵取り角
αＦ：滑り角
ＶＦ：前輪の速度
ＦＦ：前輪と大地との間の横方向力
ＦＲ：後輪と大地との間の横方向力
ｍ：ビークル質量
Ｉ：ビークル慣性モーメント
θ：ビークルヘッディング
ＣαＦ、ＣαＲ：前及び後タイヤコーナリングこわさ
【０１４６】
0003560959.tif 000022
【０１４７】
最適制御理論の分野では、システム内の選択されたパラメタを最小化するために使用される費用関数を選択しなければならないことは周知である。この問題に使用される費用関数は以下のようにして選択された。
0003560959.tif 000023
状態方程式（１４）及び費用関数（１５）を用いて最適制御問題を解くには若干の問題が存在する。
【０１４８】
１．システムは非線形である。通常、非線形がもたらす２点境界値問題は解析的解を持たない。一方、数値的解の計算には長時間を要する。
２．得られる最適制御問題は自由最終時間問題である。一般に、固定最終時間問題を解く方が、自由最終時間問題を解くよりも簡単である。
３．積分（上記費用関数内の）内の第１項は制御入力の時間導関数であり、通常は最適制御問題の２次費用関数内には存在ない。しかし、舵取りの時間レート変化は、それが遠心力（ビークルの横方向加速に起因する）の変化の時間レートに直接関係付けられていることから、滑らかな経路追尾にとって極めて重要である。
舵取り角が、図５１に示すように経路の曲率に依存することに注目されたい。上記３つの問題を解消し、得られる最適制御問題を処理し易くするために以下の方策が適用される。
【０１４９】
１．（１４）の第１及び第２式内の正弦関数がシステムを非線形にしているから、現ビークル位置に対する経路の対応点の正接方法に平行な軸を有する新しい座標系を使用する。費用関数では、偏差は横方向においてのみ考慮される。これら２つの近似は、システム方程式内の非線形を排除するだけではなく、処理する式の数を減少させ、（１４）の第１式は不要になる、（“座標系”参照）。
２．自由最終時間ｔｆを有するこの問題は、前方距離に対する運動の式内へ微分を書き込むことによって、独立変数の固定最終値を有する問題に変換することができる。この目的のために、非次元独立変数Ｓを次のように定義する。
0003560959.tif 000024
Ｓｆ＝ａｃｃｅｌ
３．上述した第３の問題を解くために、新しい状態ベクトル及び制御入力を以下のように定義する。
0003560959.tif 000025
【０１５０】
ここに、Ｘｎｅｗは以下を満足する：
0003560959.tif 000026
【０１５１】
ｙ＝Ａｘ＋Ｂ
ここに、Ａｏｌｄ及びＢｏｌｄは古いシステムマトリクス及び古い入力マトリクスを表す。
【０１５２】
次いで、状態変数及び制御入力は次の用に定義される。
0003560959.tif 000027
これは、以下のようにシステム方程式を満足する。
0003560959.tif 000028
新しい費用関数は次のようになる。
0003560959.tif 000029
上述の得られたシステム方程式（１７）及び費用関数（１８）を用いた舵取り計画は、以下のようにして線形２次追尾問題として解くことができる。システム方程式及び費用関数を以下のように記述するものとする。
0003560959.tif 000030
【０１５３】
また、Ｑｆ≧、ｏ，Ｑ≧、ｏ，Ｒ≧ｏ
は全て対称に選択される、従って得られた方程式は以下の通りである。
0003560959.tif 000031
【０１５４】
従って、ｒｉｃｃａｔｉ方程式（２１）を先ず解き、ｒｉｃｃａｔｉ方程式の結果から利得を計算し、そして所望の経路によって駆動される強制関数を式（２３）を解くことによって計算する。次いで、式（１９）及び（２４）を解くことによって制御及び状態を求める。  Ｒｉｃｃａｔｉ方程式を解くために、マックファーレン・ポッタ積分法を試みた。この方法は時間不変問題の安定状態解に対して極めて効果的であることが知られている。予測距離はかなり長く、解の初期部分を使用するから、この方法は計算時間を短縮するのに良いものと考えられる。
従って、予測距離が長く、解の初期部分だけを使用するので、式（２３）を以下の式（２５）に変形してそれを解く。
（Ａ−ＢＫ）Ｔｖ＋ＱＸｄ＝Ｏ                                                （式２５）
【０１５５】
ｈ．結論
追尾性能は、本発明によりビークル及び制御システムダイナミクスを研究し、理解することによって、及びこの理解の下に補償方法を設計することによって改善された。
追尾方法の性能の劣化には、ビークル制御命令の待ち時間、遅いシステム応答、及びビークルの動的特性が関与しているのである。これらの各効果に反作用させることが可能である。
有力な効果であるビークル命令の待ち時間は、ビークル制御ハードウェアを変更することによって、及び制御命令を十分に前広にセットして遅延を補償する方法を使用することによって成功裏に補償することができる。ビークル位置を検知する時点と、命令を発行する時点との間の時間遅れを減少させると、予測誤差が減少する。これは舵取り角を計画するために必要なことであり、これによって良好な追尾性能がもたらされる。
速度に伴って予見距離を変化させることも、予見距離を一定にする場合に比して追尾性能を改善する。
一般的に言えば経路追尾はコースに留まることの関数である、上述したように本発明の経路追尾において検討した若干の要因は、距離、ヘッディング及び曲率の誤差と、処理遅延、アクチェエータヘのビークル応答の遅延等を含むシステム内の遅延と、動的な予見距離と、重み付き経路履歴と、補外とである。
【０１５６】
Ｄ．障害物処理
１．序
障害物処理は、少なくとも３つの主要機能、即ち障害物４００２の検出と、障害物４００２の回避と、経路３３１２への復帰とを含む、経路への復帰機能は上述の経路生成及び追尾に類似する。
ビークル１０２の航行を成功させるためには、経路追尾（追随）の他に、ビークル１０２がその経路内の障害物４００２を検出し、衝突が発生する前に障害物を回避するためにビークルが停止するか、または他の回避動作を起こすことができるようにすることが要求される。
本発明の１実施例では、単一線赤外レーザスキャナ４０４（図３８参照）を、走査が水平（図示してない）に行われるような形態で使用している。走査線３８１０は大地と接しないから、距離データの何等かの不連続が環境内の物体４，００２を表すことになる。
参照経路３３１２が使用可能であり、この参照経路に対するビークルの位置は既知であるから、危険物体４００２に関しては距離データ及び参照経路３３１２を画している領域だけが処理される。この領域、または境界ゾーンの外側の物体は無視される。境界ゾーンの幅（図示してない）は、ビークル幅プラスある選択された安全緩衝に等しくして追尾及び位置特定誤差のための余裕を見込んでいる。この方法はその有用性に限界があり、“隙間検査”と呼ぶ。
【０１５７】
２．障害物の検出
ａ．隙間検査
本発明の最も簡単な例ではレーザ４０４を単一線走査で使用し、視野を規則的な角度間隔でレーザ走査して連続距離測定に成功している。これもまた簡易化のために、これらの走査を規則的な時間間隔で開始することができる。“隙間検査”なる語はこの方法を記述するために使用しているのである。本発明のこのバージョンにおけるこの方法は、二次元デー夕のみを処理することに制限される。
この型の障害物検出方法は、視野全体をスキャナ４０４が走査するに当たって連続距離測定を規則正しい角度間隔で行う単一線走査モードを使用して経路３３１２が支障があるか否かを見る検査に制限される。何等かの障害物４００２の存在を特定するか、またはもし経路に支障があればその周囲に経路を作成するような如何なる方法も含まない。この型の方法は、工場の床のような極めて厳密に制御された環境を除いて、特に有用な障害物検出方法であるとは考えられない。
【０１５８】
ｂ．濾波及び線検出計画
本発明の第２の障害物検出は、走査３８１０がビークル１０２の前方のある距離で大地に接するような多重線スキャナ３８０４（図３８参照）を使用する。走査線が大地と接するので、距離データの不連続は最早危険物体４００２を表してはいない。例えば、丘及び急傾斜した道路または路頂のような自然物の輪郭が距離データに不連続をもたらし得る。本発明のこの技術は、危検物体４００２及び自然物体（図示してない）からの距離データを識別することができる。
本発明のこの実施例では、処理されるデータの量を減少させるため濾波計画が使用され、また使用されるスキャナ構成とは無関係である。境界ゾーンの縁は距離データを、各距離値が行番号３９０８及び列番号３９１０（マトリクス表現）によって検索される画像面表現３９００（図３９参照）に転送することによって見出される。
距離画像表現３９００内で使用可能な比較的少数の走査線を選択することによって処理される負荷は最小化される。これらの走査線はビークル速度によって選択され、ビークルの停止距離及びそれを越える点に集中される。連続するデータフレームから選択される走査線は重複することができる。
この方法では、もしビークル１０２が高速で運動していれば、選択される走査線３９０６はビークルの前方（距離画像表現３９００の上方）へ離れる。これに対してビークルがゆっくり走行している場合には、選択走査線３９０６はビークルに近づく（距離画像表現３９００の下方に接近する）。
【０１５９】
各走査線はデータの多くの絵素からなる。各絵素はそれに関連する２つのパラメタを有している。第１に、絵素の実際の値はスキャナ３８０４から戻される距離値である。第２に、走査線上の絵素の位置は、その距離が記録された時のビークル中心線に対する角度の指示を与える。これは円筒座標フレーム（Ｒ、シータ、Ｚ）記述に対応する。
円筒記述及びビークル１０２に対する既知のスキャナ位置を与えれば、距離値Ｉは直交座標（Ｘ，Ｙ，Ｚ）系に変換することができる。その結果が道路輪郭記述であり、これはビークル経路３８１２内に危険な物体４００２が存在するか否かを決定し、一方で典型的な道路内の自然の丘及び谷に起因する効果を無視する新しい濾波計画に使用することができる。
【０１６０】
スキャナデータが直交座標に変換された後、データは、走査のどの部分が実際に道路３３１２上にあり、走査線のどの部分がビークル経路の外側にある（従って、安全に無視できる）かを決定するために処理される。ビークル位置及び境界の幅（ビークル幅プラスある安全余裕度に等しい）を与えることによって、ビークル経路の何れの側の境界の座標をも決定することができる。境界の座標は、現走査線上の各絵素の座標と比較することができる。境界の外側の座標を有する絵素は無視される。
濾波計画は、先に検知された道路輪郭から道路輪郭の予測を構築する。この予測は、典型的な舗装された道路を十分に記述することが見出された３つのパラメタに基づいている。
§路頂：道路断面（道路の中心線に垂直な）の曲率。
§道路傾斜：道路輪郭（道路の中心線に垂直な）の｀傾き’。
§道路高さ：ビークル１０２の４つのタイヤの位置によって記述される参照面上の道路中心線の高さ。
路頂及び道路傾斜の予測値は、先に検知したスキャナデータに対して標準の最小自乗カルマン濾波技術を遂行することによって決定される。カルマンフィルタは、基本的には、先行データから決定された値に基づいてこれら２つのパラメタのランニング平均の型を保つ。
【０１６１】
本発明によれば、特定走査に対する道路高さは２つの類似した方法の一方によって決定することができる。
第１は、現走査線内の各絵素における道路高さを平均して当該走査線の特性高さを決定する方法である。
第２の方法は、路頂及び傾斜予測を決定する時に使用したものと類似の標準カルマンフィルタを使用して道路高さを濾波することである。
これら３つのパラメタを使用して、予測される道路輪郭を記述する２次方程式を決定することができる。この予測輪郭は実際の道路輪郭と比較される。両者間に所定のしきい値を超える何等かの偏差があれば、それらは危険をもたらす物体であるものと見なされる。
【０１６２】
本発明のこの計画は、検出される物体４００２が道路の幅に比して小さいものと仮定すれば実行可能である。これらを平均化する、または最小自乗法を使用すれば、物体に起因する効果は自然の道路データに比して無視できる。
この濾波計画は、選択された距離データを簡単な７点重み付け関数を用いて畳み込む極めて簡単な線検出法をも含む。
【０１６３】
Ｃ．障害物抽出
本発明の付加的な技術は、多重線スキャナ３８０４からの物体に関する距離画像表現３９００全体を処理する。本発明のこの方法は３つの目標を達成する。
１．何も存在しない場合には障害物４００２を検出しない。
２．障害物が存在する場合には障害物４００２を検出する。
３．障害物が存在する場合には正しい障害物４００２を検出する。
障害物抽出は、ブロッブ（ｂｌｏｂ）抽出を使用する障害物検出である。ブロッブ抽出は、コンピュータグラフィックスの分野では周知である。障害物は類似絵素をブロッブと呼ぶ群にクラスタ化することによって見出される。障害物抽出の目標は、障害物を個々の絵素としてではなくユニットとして記憶し、処理することである。
【０１６４】
本発明の障害物抽出は、画像面３９０１内で以下の諸段階を遂行することによって行われる。
１．ビークル経路を画像面３９０１内へ投影する。
２．距離データを高さデータに変換する。
３．道路の中心の高さ（これは各行における予測された道路高さを表す）に曲線を当てはめる。
４．実際の道路高さと高さ予測とを比較する。
５．障害物（実際の道路高さと予測された道路高さとの差がしきい値を超えたことによって指示される）を抽出する。
【０１６５】
（１）道路の発見
使用可能な全てのデータを処理するために、画像はスキャナ３８０４のフレームレートで処理しなければならない。この理由から、障害物検出法における殆どの計算は画像面３９０１内において行われる。経路を画像内に投影することによって画像の大部分は無視することができ、多くの不要計算が回避される。
ビークル経路３８１２が規則的な間隔で指定されているものとすれば、現ビークル位置を使用してスキャナの前方の経路セグメント線３９０２を探知することができる。この経路３８１２は、道路または境界縁３９０２に対応する点を画像面３９０１（図３９参照）内へ投影することによって、実際の座標から画像座標へ変換される。
間隙の間を補間するために３次スプラインが使用される。即ち、行３９０２の中心及び縁が画像面３９０１内の各行３９０８毎に見出される。道路線３９０２間で孤立している絵素は距離から高さデータヘ（円筒座標から直交座標へ）変換される。域外の絵素は破棄され、それ以上処理されることはない。
【０１６６】
（２）道路高さのモデル化
画像面３９０１内の全ての行３９０８の道路の中心が分かると、これらの各点毎の高さを決定することができる。３次最小自乗曲線がこれらのデータに当てはめられる。
これは、道路の一般的な傾向（上り、下がり坂）をモデル化し、また道路の中心に横たわる雑音及び小さい物体の効果を濾波して除去する効果を有する。
（３）しきい値設定
障害物は、高さしきい値を使用することによって探知することができる。周囲の地形が平坦であるとは限らないから、直線的な高さしきい値は無意味である。従ってこのしきい値は、所与の絵素の行番号３９０８における３次適合によって予測される予測高さに対して参照される。
このようにすると、丘は高さ予測と実際の高さとが極めて精密に整合するから障害物とは見なされない、一方実障害物４００２は予測道路高さを殆ど反映せず（最小自乗適合のために）、従ってしきい値設定によって容易に見出される。
このしきい値設定の結果は、“ブロッブ抽出”に適する２値画像（図示してない）である。２値画像は、何処に物体があるのかを、または物体が画像内には存在しないことだけを指示する。
【０１６７】
（４）ブロッブ抽出
ブロッブ抽出は、セットされた隣接絵素（障害物４００２が存在することを指示）を一緒にクラスタ化し、それらをユニットとして処理することによって作業する。２つの絵素は、もしそれらが
１．同一列３９１０内にあり、連続行番号３９０８を有している
２．同一行３９０８内にあり、連続列番号３９１０を有している
の何れかである場合に隣接していると言う。
絵素を一緒にブロッブ内にグループ化することによって、障害物４００２を全ユニットとして処理することができ、さらなる処理に適するものとなる。
【０１６８】
（５）応用
抽出されたブロッブを使用する１つの方策は、それらを別のプログラムヘの入カとして転送することである。例えば、物体４００２は座標内ヘパーズし、グローバル目的写図４００４（図４０参照）を累積するために使用することができる。この写図４００２は別のプログラムヘ渡され、衝突回避または経路計画を行うために使用される。
（６）障害物の回避
本発明がビークル１０２の経路内に障害物４００２を検出すると（図４０参照）、その物体との衝突を回避しなければならない。障害物回避問題に関して若干の仮定を行う。
１．障害物環境は、凸多角形または凸線によって表すことができる障害物４００２が散在している。
２．航行方法は、未処理レーザ距離データから、またはブロッブ抽出を通して処理済のデータから入手できるビークル１０２の位置から見ることができる障害物の全ての面を表すローカル写図の形状のローカル環境情報へのアクセスだけを有している。
３．ビークル１０２は普通に舵取りされる型であり、その速度及び加速度に対する制約と、その舵取り角及び舵取り角の変化のレートに対する細約とを有している。障害物回避問題を処理するために、本発明はそれを２つの副問題に分割する。第１は、何等かの障害物が途上に存在するか否かを決定し、もし存在すれば、ビークルはどちら側を通り抜けるべきかを決定することである。次にビークル１０２を障害物４００２を廻って導く副目標４００６を選択し、所望の経路に戻す高いレベルの目標４００８に向けて導く。
【０１６９】
第２は、副目標４００６を選択した後に、障害物４００２を避けるように舵取りしながらビークル１０２を副目標４００６に向けて駆動する舵取り決定を行うことである。本発明の副目標選択方法及び舵取り決定方法がこれら２つの副問題を解決する。
上に列挙した仮定は以下の処理過程において管理される。
障害物探知はレーザ距離スキャナ３８０４または４０４から得られる。スキャナ３８０４または４０４が生成した距離データは、ビークル位置から見ることができる障害物の部分をモデル化する多角面のリストを作成するために処理される。新しい距離データが使用可能になるように、副目標選択方法が実行されて副目標４００６を生成し、舵取り決定方法のための安全航行の領域（自由空間４０１０）を決定する。副目標選択方法を実行することができる頻度は、スキャナ３８０４または４０４がデータを収集できるレートに依存する。達成可能なビークル速度自体は、この実行頻度に依存する。
【０１７０】
舵取り決定方法に関しては、滑らかな経路を生成するためにより高いサンプリテングレートが望ましい。従って、舵取り決定方法は副目標方法よりも屡々実行される。
副目標方法の基本的流れは以下の通りである。
１最後の初期副目標、副目標、及び自由空間を保管せよ、目標遮蔽フラグを真にセットせよ
２もし最終目標が可視であれば
直接目標を生成せよ
もし直接目標が可視であれば
目標遮蔽フラグを偽にセットせよ
３そうでなければ
初期副目標を生成せよ
副目標を初期副目標にセットせよ
【０１７１】
最新副目標が可視になるまで副目標を繰り返し生成せよ。もし副目標が実行可能でなければ、中止せよ
４もし目標遮蔽フラグが真であれば
古い初期副目標、副目標、及び自由空間を復元せよ
５そうでなければ
自由空間を生成せよ
もし自由空間が安全でなければ
【０１７２】
古い初期副目標、副目標、及び自由空間を復元せよ
副目標方法：先ず（上記段階１）、先行繰り返しから生成された初期副目標、副目標、及び自由空間を保管する。これは、新たに生成された副目標が安全ではない場合に、古い副目標を追求し続けることを可能ならしめる。
次に（上記段階２）最終目標が可視である場合には、どの障害物４００２にも関連がない直接目標を生成することを試みる。最終目標がローカル写図内で可視ではあっても、スキャナ範囲（距離的に、及び角度的に）の外側の障害物はローカル写図内には表されていないから、必ずしもこれが障害物が最終目標を遮っていないことを意味するものではない。従って直接目標を生成する場合には、ローカル写図内には存在していない障害物４００２上に、またはその背後に副目標を配置することがないように、スキャナ３８０４または４０４によってカバーされる円錐領域内に目標を位置定めするようにする。
【０１７３】
次の段階（上記段階３）は、ローカル写図内において最終目標が障害物４００２によって遮られているような状況を処理する。この場合、最終目標への視線を遮っている障害物４００２が先ず決定される。
流れ図を遮っている障害物が与えられると、それを廻って進む２つの道が考えられる。もし障害物の両縁がスキャナ３８０４または４０４の範囲内にあれば、ビークル１０２から縁までと、縁から最終距離までの距離の合計が最小になる方の縁を廻って進むように選択することができる。もし障害物４００２の一方の縁だけが範囲内にあれば、その縁を廻って進むことが選択される、もし縁が見えなければ、常に左縁を廻って進むことが独断的に選択される。廻って進む縁が決定されると、ビークルの大きさに比例する距離だけ縁から離して初期副目標が配置される。
【０１７４】
この変位のために、得られた副目標が他の障害物４００２によって遮られるかも知れない。これは、今生成された副目標への視線を遮っている障害物上に副目標を繰り返して生成することを要求する。この繰り返し処理過程は、ビークル１０２が見ることができる副目標が生成されるまで続行される。このようにして生成された各副目標は実行可能性が検査される。実行可能性とは、ビークルが通過するには小さ過ぎる２つの障害物４００２間の間隙に向かって副目標がビークル１０２を導かないことを意味する。このような状態が検出されると、ビークル１０２は停止することになろう。第２段階（上記段階２）において生成された直接目標が、ビークル１０２からは不明瞭である可能性がある。もしこれが事実であれば、先行繰り返しから古い副目標が復元され、次（上記段階４）において使用される。
最終段階（上記段階５）では、可視副目標のための自由空間４０１０（これは障害物を含まない三角形領域である）が生成される。この自由空間４０１０が生成されると、副目標及び自由空間４０１０の安全性を決定することができる。新しい副目標及び自由空間４０１０が安全でない場合には、古い副目標及び自由空間が再び復元される。そうでない場合には、新しい副目標及び自由空間が使用される。
【０１７５】
本発明の舵取り決定方法は、２つの主要成分、即ち状態制約から制御制約への移行と、所望制御ベクトルの決定とからなる。
制御制約及び所望制御ベクトルが計算されると、当分野では周知の最適化技術を使用して制御ベクトルを決定することができる。
【０１７６】
４．経路への復帰
発明は、図４０に図式的に示すように、物体４００２を回避した後のビークル１０２が参照経路を再取得するように検出した物体４００２を廻る安全な経路をプロットし、航行させる方法を含む。
【０１７７】
５．スキャナシステム
ａ．序
図３８及び４２を参照する。本発明はレーザスキャナシステム４０４をも含む。スキャナ４０４は、前述のようにビークル１０２経路内にランダムに突然出現する障害物４００２（図４０参照）を見出すために使用される。
これらの障害物４００２の源は変化するものであり、特定の作業現場に大きく依存する。これらは、落下した木及び枝、巨礫、移動中のまたは駐止中のビークル、及び人々を含むであろう。
スキャナ４０４は、自律ビークル１０２に必須条件として外部世界を検出し、処理する能力を与える。
ｂ．レーザスキャナ
レーザスキャナシステム４０４の主要成分を図４２に示す。
レーザ距離ファインダ３８０４は、距離ファインダユニット３８０４と、最寄りの物体４００２との間の距離を測定するために赤外ビーム３８１０を使用する。短いパルスがユニット３８０４から送信され、ビーム３８１０が物体４００２から反射して戻るのに要する時間が距離を与える。
距離ファインダ４０４からのビーム３８１０は回転鏡４２２２によって反射され、距離ファインダ４０４に３６０°世界の視野を与える。鏡の回転は電動機４２０６によって達成される。電動機速度は、標準のＲＳ２３２Ｃ直列リンク４２２４を通して電動機増幅器／制御装置４２２０と通信する端末４２１０によって制御される。レーザ発射と鏡の角位置との同期はエンコーダを用いて行われる。
レーザ距離ファインダ４０４からの線４２２６上の距離データはインタフェース回路４２２８によって受けられ、差動的にバッファ回路４２１４へ伝送される。データの個々の片々は、鏡４２２２が完全１回転するまでバッファ回路４２１４によって収集される。このデータ集合が１走査を構成する、ある走査が完了するとバッファ回路４２１４はプロセッサ４２１２に通知し、その走査全体のデータはプロセッサ４２１２へ転送されて処理される。
【０１７８】
Ｃ．スキャナシステムインタフェース
インタフェース回路４２２８は３つの機能を有している。
第１に、それは安全モニタとして動作する。電動機４２０６と鏡４２２２との間の駆動ベルト４２３０が切断した等によって、鏡４２２２が回転を停止する状況が発生するかも知れない。この状態の下でもレーザ４２０４は発射し続けるが、鏡４２２２が静止しているのでそれは単一の点に発射することになる（レーザビームを直接見ることは誰にとっても危険である）。しかし、インタフェース回路４２２８は、鏡４２２２の角速度が毎秒回転数の半分以下に低下したことを検知し、もしこのような状態が発生すればレーザ４２０４を作動不能にする。
第２の機能は、３６０ｏ走査領域の一部分にわたってレーザ４２０４の発射を不能にすることである。典型的には、レーザスキャナユニット４０４はビークル１０２の前部に取り付けられており、関心のある視野はビークルの前方１８０°の範囲であろう。ビークル自体も３６０°走査領域の後方部分を遮るであろう。この場合回路４２２８はレーザ４２０４がビークル内に発射することを阻止し、ビークルの前方領域の距離データを受信する間のレーザダイオードの寿命を伸ばす。レーザ距離ファインダ４２０４の動作の可能化及び不能化は、鏡ハウジング４２２２付近に取り付けられている２つのセンサ（図示してない）を介して行われる。試験の目的に対しては、または３６０°走査が望まれる応用に対しては、不能化機能はＤＩＰスイッチによって遮断することができる。
回路４２２８の第３の機能は、信号をシングルエンデッドと差動形状との間で変換することである。レーザユニット４２０４からのＴＴＬ信号は差動的にバッファ回路４２１４へ伝送され、バッファ回路４２１４から差動伝送された信号はＴＴＬレベルに変換される。これは、２つの回路を接続しているケーブル４２２６における雑音劣化を防ぐ。
【０１７９】
ｄ．スキャナシステムバッファ回路
バッファ回路４２１４の機能は、レーザ４０４の発射と鏡４２２２の角位置とを同期させて完全１回転にわたってデータを収集し、走査をコンピュータ４２１４へ伝送して処理させることである。
鏡４２２２の角位置はエンコーダ４２０８から送られる信号によって決定することができる。バッファ回路４２１４はエンコーダ４２０８からの２つの信号、即ちＺ及びＡチャネルを使用する。Ｚチャネルはエンコーダ標識であって、エンコーダ４２０８の各回転毎に１回表明され、走査領域の始まりを信号するために使用される。
Ａチャネルはエンコーダ４２０８の２線直交出力の一方の線であり、エンコーダの各回転毎に１ＯＯＯパルスを発生する。このチャネルはレーザの発射をトリガするために使用される。
走査フィールドをエンコーダ信号に完全に同期させるためには、１つの別の信号が必要である。エンコーダ／電動機４２０６と鏡４２２２との間には２：１の歯車比が存在する、エンコーダ４２０８が２回転すると鏡４２２２は１回転する。
これは、鏡４２２２が１回転する毎に２Ｚチャネルパルスと、２０００Ａチャネルパルスとに変換するが、走査の第１の半分の始まりと第２の半分の始まりとを区別することはできない。
走査フィールドを完全に同期させるためにインタフェース回路４２２２が生成するＤＢ（不感帯）信号を使用する。走査の後半におけるレーザ４２０４の発射を不能にするために使用されるＤＢ信号によって、走査の前半と後半とを区別することができる。Ｚ及びＤＢ信号は一緒になって走査領域の始まりを信号する。
完全１走査中のデータを収集するバッファ回路４２１４の第２のタスクは、エンコーダ４２０８のＡチャネルを通して達成される。チャネルの２０００パルスは回路基盤４２２８上のＤＩＰスイッチ（図示してない）によって選択された２，４，８または１６までに分割することができる。これにより走査当たりのデータ点の数を１ＯＯＯ，５００，２５０及び１２５の間で変化させることができる。分割された信号は適切な角度間隔でレーザ距離ファインダ４２０４をトリガし、また得られた距離データをメモリ４２１４内に記憶させるために使用される。
事象のシーケンスは以下の通りである。分割されたＡ信号野立ち上がり縁で１クロックサイクルのＷ（書き込み）が表明される、この時点には先行丁（レーザトリガ）からのデータが使用可能であり、メモリ４２１４内に記憶されている。Ｔは次のクロックサイクルに表明され、レーザをトリガし、得られた距離データをメモリ入力バス４２２６上に配置する、このデータは次のＷパルスで書き込まれ、サイクルを繰り返す。
バッファ回路４２１４の最終タスクは、走査データをコンピュータ４２１２へ伝送して処理させることである。完了した走査はＺ及びＤＢ信号によって信号される（ある走査の始まりは先行走査の終りでもある）。走査が完了すると割り込み要求線が表明され、鏡４２２２が半回転を行ってしまうか、またはプロセッサ４２１２が割り込みを承認するまで表明され続ける。第１の場合には鏡４２２２の半回転は次のＺパルスによって信号され、タイムアウト状態を指示する。プロセッサ４２１２は承認に失敗し、データは失われる。
【０１８０】
通常の場合には割り込みは承認される。承認を受信すると、ＳＴＲ（データストローブ）が表明され、ＩＢＦ（入力パッファー杯）を受信するまで保持される。この時間中、データはデータバス４２３０上に供給され、コンピュータ４２１２による準備が整う。データはＩＢＦが表明されるまでバス４２３０上で有効であり、１ＢＦが表明されるとＳＴＲの表明は取り消され、データはバス４２３０から除去される。プロセッサ４２１２がＳＴＲの表明取り消しを検出すると、ＩＢＦの表明を取り消す。これによりデータの次の片の間ＳＴＲを表明させ、サイクルを繰り返す。
走査データは収集され、２つのメモリバンク４２１４内に記憶される。これは共用メモリ、及び走査記憶と走査伝送との間の同期問題を回避する。新しい走査のデータは一方のバンク内に記憶され、先行走査は他方のバンクから伝送される。
バッファ回路４２１４は、プロセッサ４２１２がレーザ発見と鏡位置とを同期させる責任、及びデータの個々の片々を収集する責任から解放する。プロセッサ４２１２はデータを走査サイズの塊として受信するので、ＣＰＵ時間をより効率的に使用できる。プロセッサ４２１２はその時間をデータの処理に費やし、データの収集に費やすことはない。
【０１８１】
Ｅ．ビークル制御システム
１．序
図４３を参照する。ビークル制御は、４つの低水準機能ブロックからなる。
第１は“ビークル管理者”（４３０２）と呼ばれる。第２は“速度制御”（４３０４）と呼ばれる。第３は“舵取り制御”（４３０６）と呼ばれる。第４は“モニタ／補助制御”（２つの分離したブロック４３１０及び４３０８として示す）と呼ばれる。これらに関して以下に説明する。
これらは全て高速直列データバス４３１４に結ばれている、バス４３１４は、データ衝突検出・パケットパッシングシステムである。
これらの各機能ブロックは、個別にマイクロプロセッサ（例えばＭｏｔｏｒｏｌａ  ６８０００  １６ビットシリーズ）を有している。これらの各マイクロプロセッサはバス４３１４を通して他のマイクロプロセッサに話しかけ、他のマイクロプロセッサを聴く。各機能ブロックはある程度特定の機能を有し、ビークル管理者４３０２は通信ハブとして機能する。ビークル管理者４３０２それビークル管理者４３０２は、ＲＳ−４２２，９６００ボー直列リンク４３１６を介してナビゲータ４０６ヘメセッセージを送り、それからメッセージを受信する。またビークル管理者４３０２はＦＭ無線通信リンク４３１８を介して遠隔制御、または“テレ”パネル４１０を聴取して送信する。
２．ビークル管理者（モード）
前述のように、ビークル管理者４３０２は、遠隔制御パネル４１０及びナビゲータ４０６から命令を受信する。次いでビークル管理者４３０２は、ビークル１０２を“Ａ”（自律）、“Ｍ”（手動）、“Ｔ’（テレ）、または“Ｒ”（作動可能）のどのモードにすべきかを決定する。
ａ．作動可能モード
状態（モード）、及びビークル１０２をどのようにして状態間で変化させるか幸示す図４４を参照する。ナビゲータ４０６はそれ自身のモードを設定することはできない。ビークル１０２が、例えばテレから自律へ変化することができないことに注目されたい。この場合には、。先ず作動可能モード４４０４を通過しなければならない。
作動可能モード４４０４は、ビークル１０２を既知状態に停止せしめる。これは、例えばビークル１０２が移動している間に自律モード４４０８からテレモード４４０６へ滑らかに移行させることは困難であるからである。テレ制御パネル操縦かん４５０２、４５０４は、制御が切り換えられた時に正面位置になければならない。
テレモード４４０６から自律モード４４０８へ進む場合、ナビゲータ４０６を初期化しなければならないという配慮が存在する。例えば、ある有限の時間を要する制御を行う前にルートに対して何処にいるのかを決定しなければならない。さもなければ、この時間の間ビークル１０２は制御されずに駆動されることになりかねない。
【０１８２】
ｂ．テレモード
テレ操作モード、遠隔制御モード、または無線制御モードとも呼ばれるテレ制御モードは、ビークル１０２を視界内に保ちつつ遠隔位置からビークル１０２を制御する方策を提供する。
ショップ職員はテレ操作モード４４０６を使用して、ビークル１０２を例えばヤードヘ移動させよう。有利なことには、ショベルまたはローダー操作員がこのモードを使用してビークルを積荷または荷降ろし位置へ操縦し、自律モード４４０８が制御を再開する位置までビークルを移動させるためにも使用される。
テレ操作モード４４０６においては、自律作業現場３００の各ビークル１０２は無線制御パネル４１０上で選択されたそれ自身の独特な識別コードを有しており、正しいビークルだけとの通信及び制御が行われるようになっている。ビークル１０２は、その独特な識別コードが送信された時だけテレ操作命令４３１８に応答する。手動モード４４０２とテレモード４４０６との間のようなモード間の何等かの対立は、明白な安全上の理由から手動モード４４０２を選択することによって解消されよう。
ビークル１０２はテレモード４４０６によって既知ルートから外れて操縦できるとしても、ナビゲータ４０６はテレモード４４０６で操作中のビークル１０２が何処にいるかを記憶している。
【０１８３】
Ｃ．手動モード
手動制御モード４４０２は、ビークル１０２が例えば修理ショップ、設備ヤード等の地域に極めて接近して操縦されている場合、または修理または保守のために制御サブシステムを解除する必要がある場合に要求されよう。
この制御モードは操作員が手動制御器の何れかを動作させると呼び出されるように実現することができる、例えば、ブレーキ４７０８を踏む、シフトレバーをある所定の自律モード位置から移動させる、または操舵ハンドル４９１０を握るのような簡単な動作が、手動制御モード４４０２を希望していることとして直ちに制御システムに伝えられ、システムは直ちに手動モードヘ移る。
手動モード中、自律システムはビークルの運動を絶えず監視し、ビークル位置の更新された記録を維持するので、自律モードを望む場合、及び自律モードが望まれれば、より迅速に且つより効率的に移行することができる。
自律モード４４０８を再度望む場合には、操作員は例えばスイッチまたはレバーを自律制御モードヘ物理的に移動させることによって自律モード４４０８に係合させるような確実な動作を起こす。もし望むならば、ビークル１０２をそのままの状態にする機会を操作員に与えるために、時間遅延を組み入れることが好ましい。時間遅延の終りに、システムは点灯、警笛等のような幾つかの水準の警告を与えて、ビークル１０２の自律モードヘの移行が迫っていることを指示する。
【０１８４】
ｄ．自律モード
自律モード４４０８は作動可能モード４４０４から入る。自律モード４４０８では、ビークル１０２は自律航行システムの制御下に入る。
このモードでは、ビークル制御システムは上述のようにビークル管理者４３０２を通してナビゲータ４０６からメッセージを受信する。上述のようにビークル管理者４３０２は、基本的には、残余の制御のための通信及び命令ハブである。
ビークル管理者４３０２、及び他の機能制御ブロックは全て遮断回路４３１２と通信する。遮断回路４３１２に関しては後述する。
【０１８５】
３．速度制御
速度制御サブシステム４６００は、速度制御解析器と、機関４６１４のための閉じたループ制御４６１８と、伝導装置４６１０及びブレーキ４６０６と、速度制御システムの実時間シミュレーションモデルと、独立したビークル遮断システム４３１２に結ばれているモニタ４３１０とを含むように編成されている。これはビークル１０２上の生産システムと並列に配置されるように設計されている。
速度制御機能ブロック４３０４は、３つの基本的機能を処理する。速度制御機能ブロック４３０４は、機関４６１４のガバナを制御する。速度制御機能ブロック４３０４は、ブレーキシステム４６０６を制御する。また速度制御機能ブロック４３０４は、生産伝導装置制御ブロック４６１６を介して伝導装置４６１０を制御する。
【０１８６】
生産伝導装置制御ブロック４６１６は、図４８に示す生産システム上への自律システムの並列後設機能として速度制御ブロック４３０４によってインタフェースされる。生産伝導装置制御ブロック４６１６は、マイクロプロセッサをべ一スとするシステムであって、主として速度及びそれに相応する歯車入れ換えを監視する。
自律システム速度制御ブロック４３０４は、所望する最大歯車比を伝導装置制御ブロック４６１６へ送り込む。例えば、もしビークル１０２を１５ｍｐｈで進行させるのであれば、最大歯車比は第３歯車比（サード）であろう。生産伝導装置制御ブロック４６１６は、その歯車比を適切に得るために必要な入れ換えの全てを制御する。
ガバナ４６２６（図４６）は機関４６１６に送給される燃料の量を制御する。従ってガバナ４６２６は機関速度を制御する、自律システムは、伝導装置システムに関して説明したように、生産ガバナ制御システムと並列に後段することができる。
ブレーキシステムを図４７及び図５０に示す。この場合も自律システムを生産ブレーキシステムに後段することができる。
【０１８７】
以下に図４６，４８，４７，５０及び４９に示すビークルシステムを説明する。これらのシステムはビークル駆動列及び舵取り４９００システムに関する。
図４６を参照する。ガバナ４６２６は機関速度４２２２を制御し、機関はビークル速度４６２４を制御する。機関動力は駆動列を通して駆動輪へ伝達される。駆動列は以下のものからなる。
トルクコンバータ４６１２
伝導装置４６１０
最終駆動装置４６０８
ブレーキシステム４６０６
車輪４６０４
これらのシステムの機能は当分野では周知である。
自律制御を遂行するために、幾つかのキージステムが本発明により変更された。主システムは速度制御システム（機関速度、伝導装置、ビークル速度、及びブレーキ）及び舵取りシステムであった。各キージステムは安全尺度として手動無効化能力を有するように設計されている。如何なる場合でも、もしビークルが自律的に操作されており、操作員がビークル機能の何れか１つの制御を取り上げるものとすれば、制御は自動的に操作員に戻される。
【０１８８】
システムには、作動させると全ての電子的に制御されているシステムを作動不能にしてビークル１０２を手動制御４４０２に戻す非常時無効化ボタン（図示してない、’パニック’ボタンとも呼ぶ）をも備えている。
システムは、若干のキージステムを作動させるためのキー部分である空気圧をも検知する。もしこの圧力がある所定のしきい値を下回れば、それは問題が存在するものと見なされ、ビークル制御システムは手動制御４４０２へ復帰し、そしてビークル１０２は停止する。
図４８は機関速度を制御するために使用されるシステムを示す。このシステムは、機関速度４６２２の電子制御を無効にするために手動操作できるペダル４８０６と並列に、空気圧を調整する電子制御弁４８０８及び４８１２を使用している。圧力センサ４８０２及び機関速度センサ４６２２は電子速度制御システム４３０４のための必要フィードバックを供給する。
【０１８９】
またビークル速度を制御するためには伝導装置制御４６１６も必要である、基本的制御システムは、この目的のために使用される特定ビークル上で容易に使用可能である。
ビークル速度を調整する手段として機関速度４６２２を制御するのに加えて、ビークルサービスブレーキ４６０６を制御することも必要である。このシステムは図４７に示されており、ビークル１０２の正常停止または低速化を遂行するために必要である。このシステムは、制動力を調整するために手動操作ペダル４７０８及びレターダレバー４７１０の両方または何れか一方と並列に、電子制御空気弁４７１２及び４７１６を使用している。これらの２つの手動入力を作動させると、電子制御システムが無効になる。圧力センサ４７０２及びビークル速度センサ４６２４は、制動力を調整するための必要フィードバックを供給する。
【０１９０】
ビークルを自律的に操作するためにはビークル舵取りの制御も必要である。この機能を遂行するシステムを図４９に示す。このシステムは、ビークル舵取りリンケージに取り付けられている流体圧シリンダ４９１４及び４９１６に流れを供給するように電子的に作動可能なＲｅｘｒｏｔｈ比例流体圧弁４９１２を備えている。システムは、電子制御システムに並列な手動操作可能なハンドメータリングユニット、即ちＨＭＵをも備えている。手動システムは、安全のためにもし必要であれば、電子システムを無効にすることができる。またシステムはＨＭＵ上にスイッチ４９２０を設けてあり、手動舵取りハンドル４９１０が中心位置とは異なることを検出する。ハンドル４９１０が中心にない場合にはシステムは、システムが手動で（４４０２）操作されつつあるものと見倣してビークル１０２の自律制御を作動不能にする。
ビークル駐車ブレーキの電子制御も付加的な安全機能として含まれている。このシステムを図５０に示す。自律制御下で適切に操作させるために、駐車ブレーキは手動で’オン’位置に配置される。ビークルがステータスモード（手動４４０２、作動可能４４０４、及び自律４４０８）によって進行中は、駐車ブレーキは空気圧弁５００８を電子的に制御することによって自動的に解放される。このシステムは、ブレーキレバー解放弁５０１６及び非常時ブレーキレバー５０１４からなる手動システムと並列である。
問題に遭遇すると、ビークル１０２は自動的に手動制御下に入る。駐車ブレーキの手動設定は通常は｀オン’であるから、これが駐車ブレーキを作動させ、可能な限り迅速にビークル１０２を停止させる。
【０１９１】
４．舵取り制御
再び図４３を参照する。舵取り制御機能ブロック４３０６は、ビークルの車輪の舵取り角を制御する責を負っている。舵取り制御機能ブロック４３０６は弁４９１２へ命令を送って舵取り角を制御し、タイロッドシステム上に取り付けられているレゾルバ（図示してない）から情報を受けているので、実際の車輪の角度がとれ程であるかを知っている。
舵取り角は０．５°程度の精度で制御することができ、レゾルバの精度はそれよりやや高く、１／８°程度である。
ビークル１０２の有効寿命のある時期にレゾルバの調整が狂うかも知れない。もしこのようなことになれば、ビークルは経路３３１２を正しく追尾することはできなくなる。
しかし、ナビゲータ４０６は、ビークル１０２が所望経路３３１２からどれ程離れたかを決定するために絶えずビークル１０２を監視している（ビークル１０２は常に所望経路３８１２からある程度外れており、システムは絶えず修正している）。もしビークル１０２が、例えば数メートルのようにある距離以上に所望経路３３１２から離れれば、ナビゲータ４０６は安全上の予防措置としてビークルを停止させる。
舵取り制御システム４３０６自体も、レゾルバが正確下あることを保証するためと、受信した舵取り命令４２０が雑音または他の誤差源によって劣化していなかったこととを常に検査している。システムの付加的な検査として舵取りシミュレーションモデルも実現することができる。
自律舵取りシステム４９００は、手動舵取りシステムと並列に実現するように設計することができ、また速度制御システムと同様な技法でビークル１０２に後から設置することもできる。
【０１９２】
図４９に示すように、既存の、または生産手動舵取りシステムは、ハンドメータリングユニットＨＭＵ４９１８を回転させる手動舵取りハンドル４９１０を有している。ＨＭＵ４９１８は、車輪（図示してない）を回転させる舵取りシリンダ４９１４、４９１６への流体圧用流体の流れを制御する弁４９１２を制御する。
ＨＭＵ４９１８上のスイッチ４９２０は、舵取りハンドル４９１０の位置が中心から離れたことを、舵取りが手動制御に変化したことの指示として検出する。キャブに搭乗している操作員は単に舵取りハンドル４９１０を回転させるたけで自律舵取り制御４４０８を作動不能にすることができる。
自律舵取り制御４４０８の下では、自律舵取り制御が車輪をどの位置まで回転させようともキャブ内の手動舵取りハンドル４９１０は中心に位置している。舵取りハンドル４９１０と車輪自体との間には機械的リンケージは存在しない。
勿論ビークル１０２は、望むならばビークル上に何等の手動舵取りシステムも設けずに製造することができる。ビークルを手動で駆動するためにはテレパネル５４１０を使用することができ、または例えばある種のテレパネルをビークル１０２の側内に装着し、近接した仕事場内で無線リンクを使用することなく制御することもできる。このような状況では操作員のために折り畳み式の補助席が設けられよう。
開発された舵取りモデルを説明しておくことは本発明の理解を容易にするであろう。
【０１９３】
ａ．舵取りモデル
舵取りプランナのための基準は、図５．１に示す三輸車舵取りモデルである。このモデルによればビークルの速度には無関係に所要舵角を計算することができる。Φ＝ｔａｎ−ｌＬＣ経路
このモデルを使用するためには、所望の経路３３１２は、追随すべき経路の曲率を含んでいなければならない。曲率は曲線のその点における瞬時曲率半径の逆数である。
0003560959.tif 000032
【０１９４】
f(s) p: 位置曲線
f'(s) p: 曲線またはヘッドに対する正接
f"(s) p: その点における曲線
これは、その点における2次経路導関数にも等しい。
b．経路表現
図22-34を参照する。経路3312を追尾中の自律ビークル102の応答は、部分的に、経路3312の特性に依存する。具体的には、経路3312の曲率の連続性及び曲率の変化のレート(鋭さ)が特に重要である。何故ならば、ビークル102を所望の経路3312上に保つ上で、理想化された舵取り運動がこれらのパラメタによって支配されるからである。経路が弧及び線のシーケンスとして指定されている場合には、異なる半径の2つの弧が接続される点において曲率が不連続になる。曲率の不連続は舵取りハンドルを無限に加速する必要があるから、厄介な問題である。非O速度で、このような遷移点を通って走行するビークルは、所望経路3312に沿ってオフセット誤差をもたらす。
θ一般に、そして図33に示すように、もしポスチュア3314が4つのパラメタ、即ち若干のスプライン曲線はポスチュア連続性を保証する。しかしながら、これらのスプライン曲線は曲線に沿う曲率の線形勾配を保証しない。クロソイド曲線はそれらの曲率が曲線に沿う距離と共に線形に変化する“良好な"特性を有している。(a)弧及び直線、または(b)クロソイドセグメントからなる経路が開発されている。
曲率に不連続を有する経路はより大きい安定した状態の追尾誤差をもたらす。
これは特に、アクチュエータが低速の場合に生じ易い。
経路表現は、所望経路3312を駆動するために必要な舵角3112(図31参照)を計算するための十分な情報を含んでいなければならない。即ち、少なくとも位置、ヘッディング、曲率及び速度を含んでいなければならない。所望経路3312上の位置はポスチュア3314として定義されており、本発明ではポスチュアの構造は次のように与えられる。
C．ポスチュア定義
北:所望の北座標
東:所望の東座標
ヘッディング:所望のヘッディング
曲率:所望の曲率
速度:所望の対地速度
距離:源ポスチュアと先行ポスチュアとの間の距離
d．位置情報
位置情報3322はVPS1O00から入手され、例えば71バイトのデータである。所望経路3312を追尾するために使用される情報の構造は71バイトVPS出力の部分集合であり、以下に示すVPS短定義によって与えられる。
e.VPS短定義
時刻:gpS時刻
北:wgs84偏北距離
東:wgs84偏東距離
ヘッディング:ビークルが運動している羅針盤方向
曲率:他の変数から計算される
N速度:北速度
E速度:東速度
偏揺れ速度:ヘッディングの変化のレート
G速度:対地速度
走行した距離
f．舵取り法
舵取りプランナは所望経路3312を追尾するために必要な舵角を計算する。もしビークル102が所望経路3312上にあれば、舵角は
経路上Φ舵=f(C<所望>)=tan-1LC
である。
もしビークル102が所望経路3312から外れていれば、舵角は
経路外Φ舵=f(C〈所望>十C〈誤差>)
である。
C〈誤差〉を計算するために使用される本発明の方法は、5次法である。5次法は、所望経路3312に戻す滑らかな経路を限定する誤差空間内の5次多項式である。多項式の次数は必要データ、即ちC<誤差>及び既知の端制約によって限定される。
誤差空間内の5次多項式:
誤差
0003560959.tif 000033
【０１９５】
ｓ＝０において：
誤差（０）位置＝現所望位置一現実際の位置
誤差’（０）ヘッディング＝現所望ヘッデインダ−現実際のヘッディング
誤差”（０）曲率＝現所望曲率位置曲率−現実際の曲率
ｓ＝Ｌ（Ｌ＝予見距離）において：
誤差（Ｌ）位置＝Ｏ
誤差（Ｌ）ヘッディング＝０
誤差（Ｌ）曲率＝０
多項式誤差の係数はＬ、即ち誤差が０になる距離の関数である。
誤差（０）＝ａ０
誤差’（０）＝ａ１
誤差”（Ｏ）＝２ａ２
誤差（Ｌ）＝ａ０十ａｌＬ＋ａ２Ｌ２＋ａ３Ｌ３＋ａ４Ｌ４＋ａ５Ｌ５
誤差’（Ｌ）＝ａ１Ｌ＋２ａ２Ｌ＋３ａ３Ｌ２＋４ａ４Ｌ３＋５ａ５Ｌ４
誤差”（Ｌ）＝２ａ２＋６ａ３Ｌ＋１２ａ４Ｌ２＋２０ａ５Ｌ３
これら５つの方程式は係数ａ０、ａ１．．．ａ５について記号的に解かれる。次いで各係数は、境界条件のどの合理的集合についても容易に決定することができる。
多項式の係数が決定されると、誤差”（ｓ）をあるピックド（ｐｉｃｋｅｄ）ｓについて評価することができる。このピックドｓはｓ＝Ｏから所望経路に沿う距離に対応し、現在次のように定義されている。
Ｓ＜ピックド＞＝対地速度＊計画間隔
補正項を得るために：
Ｃ＜誤差＞＝誤差”＜（ｓピックド）曲率＞
新しい舵角を得るために：
Φ舵：ｔａｎ−１〔（Ｃ＜所望＞十Ｃ〈〈誤差＞＠ピックド＞）Ｌ〕
この計算は、現在はＯ．２５秒である各計画間隔（ｄｔ計画）に遂行される。
【０１９６】
５．モニタ／補助
図４３を参照する。モニタ／補助機能ブロック４３０８及び４３１０は、ビークル制御システムの他のブロックが遂行しない若干の雑機能を処理する。例えば機関４６１６の始動及び停止、警笛の吹鳴、ベッドの昇降、駐車ブレーキオンまたはオフの設定等がその機能である。
またモニタブロック４３１０は、バス４３１４上の他の機能ブロックが送受する命令を検査して、それらが有効か否かを調べる。もし誤差が検出されれば、モニタブロック４３１０はそれを遮断回路ブロック４３１２へ通知し、システムは後述するように遮断されることになる。
６．安全システム
ａ．序
遮断回路４３１２を含む安全システム（図４３及び５２参照）は、種々の誤差が検出されると駐車ブレーキをオンに設定することによってビークル１０２を停止させるように動作する。これによりビークル１０２は、可能な最短距離で安全停止するようになる。
駐車ブレーキは、通常は“セットまたは“オン”であるように設計され、電子回路がそれを解放するように動作するから、電子制御システムが故障するとアクチュエータ５００６への電源５２１６が遮断され、弁を作動させる電力が供給されなくなるので駐車ブレーキは“セットと呼ばれるその通常位置へ戻される。
幾つかの誤った命令を受信した時、または速度及び舵取りの両方または何れか一方のシミュレーションモデルとビークルセンサ出力４６２２及び４６２４とが受け入れ難い許容差を呈した時が、システムを遮断せしめる状態の例である。遮断システム４３１２は他の自律制御サブシステムから独立し、分離したサブシステムである（図４３及び５２参照）。
【０１９７】
ｂ．遮断制御
他のビークル制御システム機能ブロックの出力に接続されている安全システム遮断回路４３１２（図４３）の詳細を図５２に示す。
これはフェールセーフ型の設計である。これはマイクロプロセッサを全く含んでいない。これは全てハードワイヤード離散論理である。
ビークル制御システム４３１２設計の特色は、全ての機能ブロックが直列バス４３１４上の他の機能ブロックの出力内の誤りを検出できることである。従ってもしこれらの機能ブロックの１つが、別の機能ブロックが正しく機能していないことを検知すれば、それは遮断回路４３１２へ信号を送ってシステムを遮断させることができる。
例えば、速度及び舵取りブロックは各々受信した命令（ビークル管理者４３０２から受信）を調べて、命令が有効であることを確認している。また告げられたこと、即ち命令が要求していることが実行されているか否かを確認することも所定の機能である。もし否であれば、これらはシステムを遮断させる。
安全システムは、パニック停止５２０８、ブレーキペダル５２０２及び舵取りハンドル５２０６のスイッチを含む無効化用手動スイッチを含んでいる。
【０１９８】
7.バスアーキテクチャ
ビークル制御システム機能ユニット4302、4304、4306、4308、及び4310を相互接続しているバス4314は、データパケット衝突検出計画を使用するリング構造で実現された直列データ型である。
F．機能記述/方法
1．以下は、「タスク図」と名付けられた図53に示すナビゲータ406の説明である。各タスク図を以下に説明する。
a．主(エグゼクティブ)
図53の中心は“主"(エグゼクティブ)5316と名付けたタスクである。このタスク5316はタスク間通信を調整し、またナビゲータ406のために行われる高水準決定を遂行する。タスク5316が行う主な決定の1つは、システム内の他のタスクから受信したメッセージに基づいて、トラッカ5306を何時(非)係合させるかというものである。
b．モニタVEHステータス
このタスク5308は、“主'タスク5316の右上に示されている◎これはビークルポート5326を読み取り、ビークルモード変化及びナビゲータ・ビークル通信状態をエグゼクティブ待ち行列5328を介して“主"5316へ報告する。更にビークル102のステータスはグローバルメモリ構造5400(図54参照)に書き込まれる。
C．スキャナ
図53のタスク図の右下角には、障害物検出システム404からのデータを“主"5316へ通信するために設けられたスキャナタスク53n1Oが示されている。
【０１９９】
ｄ．コンソール及びコンソールパーザ
コンソール５３１２及びコンソールパーザ５３１４は、図５３のタスク図には“主”タスク５３１６の真下に示されている。これらのタスクはシステムの開発中にデバッグ用ツールとして開発されたものである。これらは、端末５３０２からのユーザ入力に従ってナビゲータ４０６状態を表示し、処理する。コンソールパーザタスク５３１４は、トラッカパラメタをセットするためにも使用される。
ｅ．指令入手
このタスク５３２０は、図５３のタスク図の左上角に示されている。これは、ホストナビゲータインタフェース５３３０の一部である。ホスト処理システム１８６からのメッセージはこのタスク５３２０によって受信され、解号される。次いで、メッセージに依存して、メッセージは“主”タスク５３１６か、または別のタスクの何れかへ通信される。次いでこの別のタスクは適切な応答をナビゲー（タ４０６からホスト処理システム１８６へ送る。
ｆ．ホストヘのメッセージ
“主”タスク５３１６の左上に示されているこのタスク５３１８は、ナビゲータ４０６からのメッセージを定型化し、それらをホスト処理システム１８６へ通信する。
ｇ．ＶＰＳ位置
このタスク５３２２は、図５３のタスク図の左側に示されている。ＶＰＳ位置タスク５３２２はＶＰＳ１０００からの（２０Ｈｚ）出力を読み取る。このデータは正しさ（例えば“検査合計”について）を検査し、もし正しければそれをグローバルメモリ構造５４００、位置バッファ（ＶＰＳ位置待ち行列）５３２２内へ配置する。このタスクは、位置障害が発生するとメッセージをＩ“主”５３１６へ送る。
ｈ．ＶＰＳポスチュア
このタスク５３２４は、図５３のタスク図の左下角に示されている。ビークルが追尾中は、このタスクはポスチュアバッファ（ＶＰＳ＿ポスチュア＿待ち行列）５３３４を維持する、このタスク（５３２４）はビークルの位置を監視し、走行方向における現ビークル位置からほぼ５０ポスチュアを、ポスチュアバッファ（３０００）内に維持する。
【０２００】
ｇ．トラッカ
図５３のタスク図の右上角に示されているこのタスク５３０６は、現位置５３３２及びポスチュアバッファ５３３４を読み取る。読み取った情報に基づき、タスク５３０６は舵取り及び速度修正４２０を計算する。このタスク５３０６はそれらをビークル１０２へ送り、それによってビークルのコースを制御する。
ｊ．ナビゲータ共用（グローバル）メモリ
ナビゲータタスク５３００に関して説明したように、ナビゲータ４０６は、種々のタスクが読み書きするグローバルメモリ構造５４００を有している。このメモリ構造５４００を図５４に示す。
図５４では、タスクは、中に特定タスクを書き入れた長円で示されている。メモリ５４００は図５４の中央部に箱の積み重ねとして示されている。非保護メモリは箱の積み重ね内に単一の箱として示されている。セマフォー保護されたメモリは積み重ねられた箱内の箱として示されている。
矢印はタスクとメモリとの間のデータ転送方向を指し示している。従って、タスクからメモリヘの書き込みは、そのタスクから当該メモリを指し示す矢印を有する線で示される。同様に、タスクによるメモリからの読み出しは、メモリから当該タスクを指し示す矢印を有する線で示される。タスクとメモリとの間に双方向データ転送が存在する場合には、線の両端に矢印が付けられている。
【０２０１】
ｋ．主（エグゼクティブ）流れ図
図５５及び５６Ａ−５６Ｄはナビゲータ主またはエグゼクティブタスク５３１６の流れ図である。
先ず図５５を参照する。これは主またはエグゼクティブタスク流の一般的な構造を示す図である。以下にナビゲータエグゼクティブタスク５３１６に関連する幾つかの流れ図を説明する。
図５５はエグゼクティブ流れ図であって５つのブロック、即ち開始ブロックであるブロック５５０２と、ナビゲータを初期化するブロック５５０４と、未処理エグゼクティブ決定であるブロック５５０６と、状態に基づく動作であるブロック５５１０とを示している。
図５５は、ナビゲータ４０６に電力が投入された（電源スイッチをオンにした）時に、エグゼクティブタスク５３１６がその機能をどのように実行するかを記述している。電力が投入されると、エグゼクティブタスク５３１６（またはエクゼクティブ）は開始ブロック５５０２から開始され、直ち５５０４に進んでナビゲータを初期化し、エグゼクティブ５３１６はナビゲータ４０６を既知の初期状態にする。ついでエグゼクティブは未処理エグゼクティブ待ち行列５５０６へ進み、複数の原始からそのメッセージ待ち行列５３２８ヘメッセージが到着するのを待機する。例えば、典型的なメッセージはホスト処理システム１８６からの情報に関する問い合わせであり得る。
【０２０２】
エグゼクティブ待ち行列５３２８にメッセージを受信すると、エグゼクティブ５３１６はエグゼクティブ決定ブロック５５０８へ進む。このブロックではエグーセクティブ５３１６は既知の技法によって一連のステータスフラグをセットする。これらのフラグはナビゲータ４０６を既知の状態、具体的にはメッセージ受信済にする。
ステータスフラグが適切にセットされると、エグゼクティブ５３１６は状態に基づく動作５５１０へ進み、受信した指令の型に従って必要動作が遂行される。
今度は図５６−５６Ｄを参照する。これらの図は、図５５の一般的構造図に示した“エグゼクティブ決定ブロック５５０８の流れを示す。
エグゼクティブタスク５３１６が開始できる種々の応答の詳細を以下に説明する。エグゼクティブ待ち行列５３２８内に予測される既知のメッセージの集合が存在する。これらのメッセージの詳細を図５６Ａ−５６Ｄに示す。
【０２０３】
図５６Ａは、図５６Ａ−５６Ｄの編成図である。図５６Ａ−５６Ｄは、エグゼクティブ５３１６が種々のメッセージに対して応答するために使用される手順の詳細を記述している。図５６Ａを参照して、特定のメッセージに対するエグゼクティブ５３１６の動作を説明する。エグゼクティブ待ち行列５３２８へのメッセージを受信すると、プログラムはブロック５５０６を去ってブロック５６０２へ進み、エグゼクティブ５３１６はそのメッセージが’新ルート指令’か否かを決定する。もしそのメッセージが｀新ルート指令’であれば、エグゼクティブ５３１６は｀新ルート指令’に基づく動作ブロック５６０４へ進む。｀新ルート指令’メッセージに特定の動作が成功裏に完了すると、エグゼクティブ５３１６は状態ブロックに基づく動作５５１０へ進む。この動作が完了すると、エグゼクティブ５３１６は未処理エグゼクティブ待ち行列ブロック５５０６へ戻り、別のメッセージを待機する。もしブロック５６０２内の初期メッセージが’新ルート指令’でなければ、エグゼクティブ５３１６はブロック５６０６へ進んでメッセージが’速度変更指令’であるか否かを決定する。
【０２０４】
’速度変更指令’、’ＶＥＨ応答’、｀ＶＥＨ無応答’及び｀ＶＥＨ検査合計誤り’のようなメッセージに対する応答は、’新ルート指令’に関して記述した手順と類似手順を辿。しかしながら、’．．．’に基づく動作１０  ５６０４乃至５６２０において遂行される動作は異なる考え得るメッセージごとに異なる。有効メッセージの種々の型、及びその要約記述は以下の通りである。
「新ルート指令」：ビークルが追随するルート番号をセットせよ。
「速度変更指令」：ルートの特定部分をビークルが走行するこができる最大可能な速度を命令せよ。
「ＶＥＨ応答」：ビークルは適切に命令に応答しつつあり、ナビゲータステータフラグを健全」にセットせよ。
「ＶＥＨ無応答」：ビークルは命令に応答しておらず、ビークルを停止させよ。
「ＶＥＨ検査合計誤り」：ビークルはデータを正しく送信／受信しておらず、ビークルを停止させよ。
「テレ」、「手動」、「作動可能」、または「自律」：ビークルのモードを正しい順序で」にセットせよ。
「ＶＰＳタイムアウト」：ＶＰＳはデータを送信しておらず、ビークルを停止させよ。
「ＶＰＳ検査合計誤り」：ＶＰＳは誤転送されたデータを送信しており、ビークルを停止させよ。
「ＶＰＳポスチュア作動可能」：経路ポスチュアの生成作動可能。
「ＶＰＳ位置作動可能」：ＶＰＳデータは使用可能。
「ＶＰＳ位置整列」：ＶＰＳは初期化中、ビークルを運動させるな。
「ルートの終り」：ビークルは現ルートの終りに接近しつつあり、到達したならばホスト処理システムに通知せよ。
「走査作動可能」：走査システムは経路内の物体の探査作動可能。
「走査無妨害」：ビークル経路上に物体は検出されず、正常に続行せよ。
「走査障害物」：ビークル経路上に物体を検出、ビークルを停止させよ。
「トラッカコース外」：ビークルは所望経路を許容差以内で追随しておらず、ビークルを停止させよ。
「トラッカルートの終り」：トラッカは経路の終りに到達した、ビークルを停止させよ。
「トラッカ停止」：追尾タスクがビークルを停止させたことをナビゲータに通知せよ。
’テレ’、’手動’、’作動可能’、及び｀自律’に対する応答は、これらのメッセージが互いに関係付けられていて、特定の順序に基づいて動作しなければならないからやや異なっている。これは既に説明済である。これらのメッセージのためのプログラム流を図５６Ａ及び５６Ｂに示し、ブロック５６２２−５６３０について説明する。
【０２０５】
爾後に見込まれるメッセージに対する応答は、図５６Ｂ乃至５６Ｄのブロック５６３２乃至５６７８に示されている。これらの応答はメッセージ’新ルート指令’に関して説明したものと類似している。
もし受信したメッセージが予測したメッセージの１つではないか、またはそのメッセージが誤転送されたものであれば、エグゼクティブ５３１６はブロック５６８０に導かれ、そこでホスト処理システム１８６は問題を知らされる。次いでエグゼクティブ５３１６はエグゼクティブ待ち行列５５０６へ戻り、待ち行列内の次のメッセージに応答する。
図５７Ａ乃至５７Ｒは、エグゼクティブ５３１６が特定のメッセージに応答するために使用する特定手順を示す。例えば、図５７Ａはエグゼクティブ５３１６が’新ルート指令’メッセージに如何に応答するかの詳細を示す。このメッセージがエグゼクティブ待ち行列５３２８内に到着すると、エグゼクティブ５３１６は流れ図ブロック５７０２へ進んでメッセージが何であるか（この場合には’新ルート指令’）を決定する、もしメッセージが’新ルート指令’であれば、エグゼクティブ５３１６は流れ図ブロック５７０５へ進み、メッセージに応答する。そうでなければ、流れ図ブロック５７０４へ進んでそれが有効（他の考え得るメッセージの１っ）であるのか、または無効であるのかを決定する。
メッセージが’新ルート指令’であることが分かると、エグゼクティブ５３１６は図５７Ａに示されている処理過程を辿ってそのメッセージに応答する。この処理過程はブロック５７０６乃至５７１４によって示されている。この手順では（そして他の指令に対する応答では）、エグゼクティブ５３１６はナビゲータ４０６内の異なるタスクの状態を検査し、これらの状態に対して既知の、所定技法で応答する。
この応答の効果は、一連のステータスフラグをセットすることであり、これによってエグゼクティブ５３１６が状態に基づく動作５５１０に到達した時にナビゲータ４０６内の他のタスクによる爾後の応答が遂行される。ブロック５５１０内に実現されている実際の手順を図５８に示す。
【０２０６】
他の有効メッセージに対するエグゼクティブ５３１６の応答は、’新ルート指令’において説明した手順に類似している。指令に対する各応答の効果は、先ずフラグの集合を変化させ、それによってナビゲータ４０６の状態に影響を与えることである。特定のフラグ集合は、特定の指令に依存する。ナビゲータ４０６は、エグゼクティブ５３１６が「状態に基づく動作」ブロック５５１０まで移動した時に、これらのフラグの変化に応答する。
図５８Ａ−５８Ｃは、’状態に基づく動作’ブロック５５１０の流れを示す。状態に基づく動作ブロック５５１０を図５８Ａ−５８Ｃに示す。図５８は図５８Ａ−５８Ｃの相互関係を示し、これらの３つの各図は状態に基づく動作ブロック５５１０の部分を示す。
エグゼクティブタスク５３１６が特定のエグゼクティブメッセージに応答して適切なフラグをセットすると、エグゼクティブ５３１６は適切なタスクまたはエンティティに対してメッセージを送るようになる。これらのタスクまたはエンティティはエグゼクティブメッセージの結果としてナビゲータ４０６システムに対する変化を知らされなければならない。
例えば、エグゼクティブタスク５３１６がエグゼクティブ決定５５０８を去ると先ず状態に基づく動作ブロック５５１０に入り、ビークルが自律モードに対して作動可能である（例えば、ＶＰＳが作動可能であり、ビークルが適切に通信し、適切なルートが命令されており、そしてビークルが自律モードで作動可能である）というようなステータスがセットされているか否かを検査する。ブロック５８０２を参照されたい。もしこれらの条件の１またはそれ以上が満足されなければ、エグゼクティブは別の有効メッセージを待機するように戻る。もしこれらの全戸での条件が満たされれば、エグゼクティブ５３１６は、経路生成器５８０４が作動中であるか否かを検査する。もし作動中であれば、エグゼクティブ５３１６は自律動作のために必要な他のシステムを開始させる。
もし経路生成システムが作動中ではなければ、エグゼクティブ５３１６タスクは経路生成器を始動させるためにメッセージ’ＶＰＳポスチュア係合’をＶｐｓポスチュア待ち行列５３３４へ送る。次いでエグゼクティブタスクは未処理エグゼクティブ待ち行列５５０６へ戻り、ビークル１０２の適切な動作を保証するために別の指令を待機する。
【図面の簡単な説明】
【図１】本発明の好ましい実施例の高水準ブロック線図100を示し、
【図１Ａ】6つの軌道面174-184内に配分された21個の機能GPS衛星130-170と、3個の予備GPS衛星(図示してない)とからなるNAVSTAR GPS内の機能GPS衛星の高水準ブロック線図100Aであり、
【図２】NAVSTAR GPSの4個のGPS衛星に関する4つの連立航法方程式を示す図であって、これらの方程式はGPS衛星200-206とビークル102との間の時計バイアスCbを含み、
【図３】は本発明を実現し、実施できる典型的な自律作業現場の高水準ブロック線図300であり、
【図４】本発明のナビゲータ406、ビークルVPSアーキテクチャ1000、及びビークル制御装置408の間の相互関係の高水準ブロック線図400であり、
【図５】本発明による自律制御システム内の種々の要素の脈絡と、それらの相互関係とを示す高水準ブロック線図500であり、
【図６】ビークル102の位置を正確に決定するために、GPS衛星星座200、202、204及び206を含み、また擬似衛星105及びべ一スステーション188と共に使用される多分NAVSTAR GPSであるGPSの動作の高水準ブロック線図600であり、
【図７】好ましい実施例のGPS処理システムの電気的アーキテクチャ/ハードウエア700を示す低水準ブロック線図であり、
【図８】好ましい実施例の図7に示すようなGPS処理システム700におけるソフトウエアの機能を示す低水準流れ図800であり、
【図９】好ましい実施例では走行距離計902及び慣性参照ユニット(IRU)904を含むMPSのアーキテクチャ/ハードウエア900を示す中間水準ブロック線図であり、
【図１０】好ましい実施例におけるVPSのVPSアーキテクチャ/ハードウエア1000を示す中間水準ブロック線図であり、
【図１１】図10のVPSアーキテクチャ1000の低水準ブロック線図1100であり、
【図１２】図10のVPS主(I/O)プロセッサ1O02の中間水準ブロック線図1200であって、VPSカルマンフィルタ1202及び重み付きコンバイナ1200を示し、
【図１２Ａ】好ましい実施例のスーパーカルマンフィルタ1200Aの高水準ブロック線図であり、
【図１３】本発明の好ましい実施例において第1位置推定の精度を改善するための星座効果方法の流れ図1300であり、
【図１４】4つのGPS衛星(図示してない)のGPS衛星星座に関する一組の計算された推定擬似距離1404、1406、1408、及び1410を示す座標系1402上の極プロット1400であって、陰影領域1412は擬似距離1406及び1408を生じさせたGPS衛星を顧慮した時のビークルの考え得る位置推定を示し、
【図１５】好ましい実施例の原形バイアス技術の流れ図1500であり、
【図１６】好ましい実施例の放物線バイアス技術の流れ図1600であり、
【図１７】好ましい実施例のべ一ス残留バイアス技術の流れ図1700であり、
【図１７Ａ】好ましい実施例のべ一ス相関器バイアス技術の流れ図1700Aであり、
【図１８】将来衛星位置の予測のための好ましい実施例における方法の流れ図1800であり、
【図１９】本発明の重み付き経路履歴技術の流れ図1900であり、
【図２０】ビークル102の第1位置推定の高水準図形表示2000であって図19に示す重み付き経路履歴方法は第1位置推定2010がビークル経路と全く一致しないためにそれを排除するであろうことを示し、
【図２０Ａ】図19及び図20に示す重み付き経路履歴技術を実現するための方法の高水準流れ図2000Aであり、
【図２１】本発明の反選択可用性技術の流れ図であり、
【図２２】本発明によるノード及びセグメントを使用するビークルルート限定の図2200であり、
【図２３】ポスチュア及び関係付けられた円が如何にして目標点から求められるかを図式的に示す図2300であり、
【図２４】最初のクロソイドセグメントの符号を如何にして決定するかを示す図2400であり、
【図２５】最後のクロソイドセグメントの符号を如何にして決定するかを示す図2500であり、
【図２６】クロソイド曲線を図式的に示す図2600であり、
【図２７】近似フレネル積分を計算するための数値的方法の流れ図2700であり、
【図２８】経路の再計画を示す図2800であり、
【図２９】二次、三次、及び四次のBスプライン曲線の図2900であり、
【図３０】本発明のポスチュア・リングバッファの実施例を示す図3000であり、
【図３１】本発明の好ましい実施例の経路追尾制御アーキテクチャ/ハードウエアの高水準ブロック線図3100であり、
【図３２】舵取り計画サイクルにおける関連ポスチュアを示す図3200であり、
【図３３】曲率を含む誤差ベクトルを如何に計算するかを示す図3400であり、
【図３４】曲率を含む誤差ベクトルを、包含されたビークル経路を用いて如何に計算するかを示す図3400であり、
【図３５】本発明のナビゲータ406の脈絡図3500であり、
【図３６】本発明の経路追尾構造の脈絡図3600であり、
【図３７Ａ】それぞれナビゲータ406のデータ流概要3700A-3700Dであり、
【図３７Ｂ】それぞれナビゲータ406のデータ流概要3700A-3700Dであり、
【図３７Ｃ】それぞれナビゲータ406のデータ流概要3700A-3700Dであり、
【図３７Ｄ】それぞれナビゲータ406のデータ流概要3700A-3700Dであり、
【図３８Ａ】ビークル搭載スキャナ404の図3800Bであり、
【図３８Ｂ】障害物4002に対する自律ビークル走査102を示す図3800Bであり、
【図３９】本発明のレーザ走査システムにおける選択された走査線3904及び3906の図3900であり、
【図４０】02を回避する自律ビークル102を示す図4000であり、
【図４１】本発明の好ましい実施例による障害物処理の図4100であり、
【図４２】本発明の好ましい実施例における障害物検出に使用されるレーザスキャナシステムの中間水準ブロック線図4200であり、
【図４３】本発明の自律採鉱用ビークルのための制御システムの中間水準ブロック線図4300であり、
【図４４】図43の制御システムの動作モード間の遷移を示す状態図4400であり、
【図４５】好ましい実施例の遠視線遠隔制御システムの高水準ブロック線図4500であり、
【図４６】好ましい実施例の速度制御4304の高水準ブロック線図4600であり、
【図４７】好ましい実施例の速度制御4304のサービスブレーキ制御回路の高水準ブロック線図4700であり、
【図４８】好ましい実施例の速度制御4304のガバナ制御回路の高水準ブロック線図4800であり、
【図４９】本発明の好ましい実施例の舵取り制御システムの舵取り制御回路4306の高水準ブロック線図4900であり、
【図５０】本発明の速度制御430の駐車ブレーキ制御回路の高水準ブロック線図5000であり、
【図５１】本発明の航法システムを開発するために使用される三輪車舵取りモデルの高水準ブロック線図5100であり、
【図５２】本発明の遮断回路の実施例を示す中間水準ブロック線図5200であり、
【図５３】ナビゲータ406のタスクを示す低水準通信図5300であり、
【図５４】ナビゲータ406のナビゲータ共有メモリの実施例を示す中間水準通信図5400であり、
【図５５】実行判断に関する高水準監視流れ図5500を示し、
【図５６】図56は図56A-56Dの実行流れ図5600A-5600Dの関係/接続の中間水準流れ図5600であり、
【図５６Ａ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５６Ｂ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５６Ｃ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５６Ｄ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ａ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｂ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｃ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｄ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｅ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｆ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｇ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｈ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり
【図５７Ｉ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｊ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｋ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｌ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｍ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｎ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｏ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｐ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｑ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５７Ｒ】各々が監視流れ図5600A-5600Dにおける“…に基づく動作"を示す低水準流れ図5700A-5700Rであり、
【図５８】それぞれ、各実行流れ図5700A-5700Qにおける”状態に基づく動作"を示す低水準流れ図5800A-5800Cである。
【図５８Ａ】それぞれ、各実行流れ図5700A-5700Qにおける”状態に基づく動作"を示す低水準流れ図5800A-5800Cである。
【図５８Ｂ】それぞれ、各実行流れ図5700A-5700Qにおける”状態に基づく動作"を示す低水準流れ図5800A-5800Cである。
【図５８Ｃ】それぞれ、各実行流れ図5700A-5700Qにおける”状態に基づく動作"を示す低水準流れ図5800A-5800Cである。
【図１】
0003560959.tif 000034
【図１Ａ】
0003560959.tif 000035
【図２】
0003560959.tif 000036
【図３】
0003560959.tif 000037
【図４】
0003560959.tif 000038
【図５】
0003560959.tif 000039
【図６】
0003560959.tif 000040
【図７】
0003560959.tif 000041
【図８】
0003560959.tif 000042
【図９】
0003560959.tif 000043
【図１０】
0003560959.tif 000044
【図１１】
0003560959.tif 000045
【図１２】
0003560959.tif 000046
【図１２Ａ】
0003560959.tif 000047
【図１３】
0003560959.tif 000048
【図１４】
0003560959.tif 000049
【図１５】
0003560959.tif 000050
【図１６】
0003560959.tif 000051
【図１７】
0003560959.tif 000052
【図１７Ａ】
0003560959.tif 000053
【図１８】
0003560959.tif 000054
【図１９】
0003560959.tif 000055
【図２０】
0003560959.tif 000056
【図２０Ａ】
0003560959.tif 000057
【図２１】
0003560959.tif 000058
【図２２】
0003560959.tif 000059
【図２３】
0003560959.tif 000060
【図２４】
0003560959.tif 000061
【図２５】
0003560959.tif 000062
【図２６】
0003560959.tif 000063
【図２７】
0003560959.tif 000064
【図２８】
0003560959.tif 000065
【図２９】
0003560959.tif 000066
【図３０】
0003560959.tif 000067
【図３１】
0003560959.tif 000068
【図３２】
0003560959.tif 000069
【図３３】
0003560959.tif 000070
【図３４】
0003560959.tif 000071
【図３５】
0003560959.tif 000072
【図３６】
0003560959.tif 000073
【図３７Ａ】
0003560959.tif 000074
【図３７Ｂ】
0003560959.tif 000075
【図３７Ｃ】
0003560959.tif 000076
【図３７Ｄ】
0003560959.tif 000077
【図３８Ａ】
0003560959.tif 000078
【図３８Ｂ】
0003560959.tif 000079
【図３９】
0003560959.tif 000080
【図４０】
0003560959.tif 000081
【図４１】
0003560959.tif 000082
【図４２】
0003560959.tif 000083
【図４３】
0003560959.tif 000084
【図４４】
0003560959.tif 000085
【図４５】
0003560959.tif 000086
【図４６】
0003560959.tif 000087
【図４７】
0003560959.tif 000088
【図４８】
0003560959.tif 000089
【図４９】
0003560959.tif 000090
【図５０】
0003560959.tif 000091
【図５１】
0003560959.tif 000092
【図５２】
0003560959.tif 000093
【図５３】
0003560959.tif 000094
【図５４】
0003560959.tif 000095
【図５５】
0003560959.tif 000096
【図５６】
0003560959.tif 000097
【図５６Ａ】
0003560959.tif 000098
【図５６Ｂ】
0003560959.tif 000099
【図５６Ｃ】
0003560959.tif 000100
【図５６Ｄ】
0003560959.tif 000101
【図５７Ａ】
0003560959.tif 000102
【図５７Ｂ】
0003560959.tif 000103
【図５７Ｃ】
0003560959.tif 000104
【図５７Ｄ】
0003560959.tif 000105
【図５７Ｅ】
0003560959.tif 000106
【図５７Ｆ】
0003560959.tif 000107
【図５７Ｇ】
0003560959.tif 000108
【図５７Ｈ】
0003560959.tif 000109
【図５７Ｉ】
0003560959.tif 000110
【図５７Ｊ】
0003560959.tif 000111
【図５７Ｋ】
0003560959.tif 000112
【図５７Ｌ】
0003560959.tif 000113
【図５７Ｍ】
0003560959.tif 000114
【図５７Ｎ】
0003560959.tif 000115
【図５７Ｏ】
0003560959.tif 000116
【図５７Ｐ】
0003560959.tif 000117
【図５７Ｑ】
0003560959.tif 000118
【図５７Ｒ】
0003560959.tif 000119
【図５８】
0003560959.tif 000120
【図５８Ａ】
0003560959.tif 000121
【図５８Ｂ】
0003560959.tif 000122
【図５８Ｃ】
0003560959.tif 000123