JP6905841B2 - ナビゲーションシステム及び誤差補正のための方法 - Google Patents

Description

測定システムの出力の誤差を補正する方法、及び統合航法システムである。

本発明は、ベース測定システム（基礎測定システム、base measurement system）と、当該ベース測定システムの少なくとも誤差を補正する補正システムと、を備えるシステムの出力の誤差を補正する方法に関する。本発明は、また、そのような方法を実行するように適合された統合航法システム（integrated navigation system）に関する。

センサ測定に基づいて所望の値や情報あるいは推定（例えば物体の動き）を算出する多くのシステムは、センサや処理動作からの入力値における不確実さや誤差に起因して、算出結果における有意な不正確さに遭遇する。これらの不正確さや入力誤差を補正すべく、参考測定又は補正測定として他のセンサから導出される付加情報を用いることが知られている。多くの場合、このような手法により、高速で、扱いやすく、常時使用が可能で、且つ処理しやすい測定手法を用いるベースシステムを、結合することができる。その処理結果は、次に、より正確な結果を出力し得るものの例えばその処理により多くの時間を要し、従って必ずしも任意の状況において使用可能とはならない追加測定値と、結合され得る。さらに、ある時刻においては、これらの追加測定が全く得られないか又は遅延だけを含む形では得られない、とうい場合もあり得る。すなわち、正確さの劣る測定を補正すべく、より正確な測定が時々使用されるに過ぎない。そのようなアプローチの顕著な例は、いわゆる統合航法システム（ＩＮＳ、Integrated Navigation System）において見ることができる。そのような統合航法システムは、通常、３次元の加速度と角速度とを測定する慣性測定ユニット（ＩＭＵ、Inertial Measurement Unit）を備える。これらの測定結果は、次に、ストラップダウン・アルゴリズム（ＳＡ、Strap-down Algorithm）により更に処理され、例えば車両の又は人が手にしているナビゲーションデバイスの、少なくとも速度、姿勢（pose）、位置が算出される。ＩＭＵとＳＡとを結合したものは、ベースナビゲーションシステム（ＢＮＳ、Base Navigation System）とも称され得る。このベースナビゲーションシステムは、上述したベース測定システムの一つの例である。

そのようなシステムでは、３次元の加速度と角速度の測定が、もちろん、常に使用可能となっている。統合航法システムが搭載された車両やデバイスの動き及び又は動きの変化を認識することが、直接的に可能なためである。ただし、そのようなシステムに用いられるセンサは、例えばオフセットなどの誤差の影響を受けることが多く、また、ベース測定センサデータを処理するストラップダウン・アルゴリズムは、センサデータの更なる処理を行う際に更なる誤差を生じさせ得る内部変数やパラメータを用いている。そのような内部ストラップダウン・アルゴリズム誤差は、例えば、センサのオフセットが積分されることで生じ得る。これらは、センサから出力された、上述の如く未補正のオフセットを含む値を、積分することにより生ずる。ナビゲーションシステムにおける一つの例は、速度を算出するための、加速度測定値の積分である。すなわち、満足な精度の位置、速度、及び姿勢を得るためには、出力エレメントの誤差についてのそのようなナビゲーションシステムの補正の結果が必要である。この分野において公知である一つのアプローチは、例えば拡張型誤差状態空間カルマンフィルタ（extended error state space Kalman-filter）などの、融合フィルタ（fusion filter）の実施である。そのようなカルマンフィルタ（又はその他のフィルタタイプ）により、ベースナビゲーションシステムの結果と参照測定との差を利用して補正値が算出される。そして、当該補正値は、ナビゲーションシステム出力又はセンサオフセットを補正すべく、ベースナビゲーションシステムにフィードバックされ得る。参照測定又は補正測定は、やや正確な位置を出力するＧＰＳ（全地球測位システム、global positioning system）により与えられ得る。ただし、ＧＰＳは、もちろん、常に使用できるとは限らず、例えば車両がトンネル内にいる場合や、他の何等かの理由により実位置の算出に必要な衛星が遮られている場合には、常に使用することはできない。他の補正測定は、例えばオドメトリを用いることにより与えられ得る。

一般に、そのような参照測定も、もちろん、誤差の影響を受け得る。特に、ＧＰＳシステムを使用する場合、例えば衛星信号のマルチパス受信（multipath reception）などの、ランダム誤差について言及しておく必要がある。そのような誤差は、さらに、カルマンフィルタの基本的な仮定に反する。そのような誤差は、ゼロ平均を持たず、従ってガウス分布ではないためである。

これまで、既に、誤差のある測定値の検出及び補正を改善するための種々のアプローチが書き表されている。

しかしながら、そのようなシステムの誤差補正を改善しようとする際に考慮すべき他の側面も存在する。発生し得る誤差は、偶然誤差（ランダム誤差）又は系統誤差（システマティック誤差）のいずれかであり得る。そのような系統誤差が発生する理由は、補正計算の実行プロセスにおいて、補正アルゴリズムに必要なデータのいくつかが遅れ得ることである。特に、ほとんどのアプリケーションにおいてこれは真実であり、統合航法システムにおいても、補正測定の結果が、ストラップダウン・アルゴリズムの結果及びＢＮＳセンサデータのかなり後に利用可能となり得る。すなわち、補正が、対応する補正センサデータが利用可能になるとすぐにデータに基づいて実行される場合、使用データの時間的不一致（時刻不一致、time inconsistency）により系統誤差が生ずる。補正データが検知された時刻と、ストラップダウン・アルゴリズムに用いられるベースシステムの最新の検知が出力される時刻との間で、車両は既に移動しているためである。一般的なベース測定システムの例であるそのようなベースナビゲーションシステムでは、通常、データは、ＩＭＵ（慣性測定ユニット、Inertial Measurement Unit）を用いて取得される。ＧＮＳＳシステム（全地球航法衛星システム、Global Navigation Satellite System）を用いて補正値算出の基礎となるデータを取得する場合、受信した衛星信号の処理には、ＩＭＵのデータの処理よりも多くの時間が費やされる。したがって、衛星信号のデータ処理結果が補正システムの入力として利用可能となったときには、既に、ストラップダウン・アルゴリズムにより、より新しいＩＭＵのデータの処理が完了しているので、時間差（時間遅延（time lag））が存在することとなる。すなわち、補正システムの入力データは、別の時刻に基づくものとなっている。対応する検知データの時刻の差は、“遅延時間（lag time）”とも称される。

これまで、ベースナビゲーションシステムなどのベース測定システムの出力側で誤差を最小化するための多くの試みがなされてきた。もちろん、一つのアプローチは、最も長い遅延時間を持つ測定値を含むすべての測定値が到着するまで待つことであり得る。このアプローチは、実際にオフラインシステムには利用可能であるものの、容易に理解され得るように、リアルタイムの用途に対しては実用的には不向きである。そのようなリアルタイムの用途では、ＢＮＳの最新の検知データができるだけ迅速に処理され、且つ出力結果が更なる処理又は分析に即座に用いられ得ることが必要である。フィルタ遅延（filter lag）を除去するフォワード・アンド・バックワード処理（forward-and-backward processing）のようなアプローチも、リアルタイム処理には適していない。これらは、もっぱら記録されたデータを処理するものだからである。

他方で、リアルタイム用途では、もちろん、計算負荷が適度なレベルに維持されることも必要である。例えば、非特許文献１に提案されているアプローチでは、遅延時間の間は誤差は変化しないという仮定の上での誤差計算を基礎に置くことが提案されている。その結果、遅延した補正測定も考慮され得る。すなわち、現在のＢＮＳセンサデータについて用いられるべき誤差の計算に際し、補正システムの遅延した測定値を用いることが可能である。しかしながら、提案されているこのアプローチは、非同期マルチセンサ環境（asynchronuous-multi-sensor environments）を扱うには不向きである。遅延時間の間は、利用可能な更なる“おそらく遅れた（possibly also delayed）”測定値は無い、ということが更に仮定されるためである。マルチセンサ環境では、この仮定は成り立たない。

他のアプローチは、他の遅延した測定値が利用可能になったらすぐに、補正アルゴリズムの全体を繰り返し、従って、フィルタ結果を再計算することを提案している。しかしながら、これは、計算負荷が高まることとなり、フィルタ結果が出力された後であってもフィルタ結果を再計算するため、時間的な不一致（temporal inconsistencies）を生じさせることとなる。

他のアプローチは、非特許文献２に開示されている。ここでは、補正システムの融合フィルタにより推定された誤差の変化は遅延時間の間は無視できる、という基本仮定が置かれる。ベース測定システム（所与の例では、ベースナビゲーションシステム）の出力は、継続的に計算され、予想される最大遅延時間の間、保存される。すなわち、遅延した補正測定値が利用可能な場合には、補正の基礎である検知データの年齢（age）が、例えば測定値のタイムスタンプを用いて特定され得る。したがって、それらのタイムスタンプに関して対応するデータのみが、融合フィルタへの入力にも用いられ得る。ベース測定システムの対応するデータが直接的に利用できない場合には、そのようなデータが内挿され得る。そのような対応するデータに基づいて算出された補正値は、次に、ベースナビゲーションシステムの出力値に適用されるが、既に保存された値にも適用される。これは、すべて、値が保存される時間期間の間は誤差の変化は生じない、という仮定に基づいている。保存された値は将来の補正値計算の基礎となるので、既に実行された補正は、すべての将来の補正計算に含まれることとなる。したがって、この方法は、マルチセンサ環境に用いることができる。このアプローチの態様は、特許文献１及び特許文献２にも開示されている。ただし、そうではあるが、上述のアプローチは、必ずしも保存された全ての測定値について一定（固定）の補正値（a constant correction value）が適切であるとは限らない、ということを考慮していないため、なおも主要な欠点が存在する。これは、例えば、速度を計算するためにオフセットを持った加速度計のセンサデータを積分する場合のように、時間経過と共に合算（積算）されたいくつかの誤差が存在するためである。そのような積分された又は合算された誤差は、もちろん、明らかに一定誤差の仮定に反する。

"Corporation of time-delayed measurements in a Discrete-Time-Kalman-filter", 198 by T.D. Larsen et.al "Eine Architektur zur Schatzung kinematischer Fahrzeuggroben mit integrierter Qualitatsbewertung durch Sensordatenfusion" by Dr. Nico Steinhardt

独国特許出願第１０ ２０１２ ２１６ ２１５（Ａ１）号明細書 独国特許出願第１０ ２０１４ ２１１ １６６（Ａ１）号明細書

本発明は、この特定の課題を考慮して、ベース測定システムと補正システムとを備える測定システムの誤差補正を改善するためになされたものである。特に、この誤差補正は、ベース測定システムのセンサデータの利用可能時期及び出力のベース測定システムの計算の利用可能時期に比べて補正測定値の利用可能時期が遅延する事象を扱わなければならないシステムにおいて、改善される。

本発明は、ベースナビゲーションシステムと補正システムとを備える統合航法システムに関する。ベースナビゲーションシステムと補正システムとは、本発明に従う誤差を補正するための方法を実行するよう適合されている。測定システム出力の誤差を補正する本方法は、まず、ベースシステムのセンサデータ（ベースシステムセンサデータ）を取得して、これらのセンサデータを処理アルゴリズムに与える。ベースシステムセンサデータは、検知が行われた時刻を示すタイムスタンプに関連付けて保存される。処理アルゴリズムにより、ベースシステムの出力値（ベースシステム出力値）が算出される。算出されたベースシステム出力値は、保存される。再び、ベースシステム出力値に関連付けて、算出されたベースシステム出力値の基礎となったセンサデータが収集された時刻を示すタイムスタンプが、保存される。さらに、補正システムは、それらの検知の時刻を示すタイムスタンプが関連付けられた補正システムのセンサデータ（補正システムセンサデータ）を取得する。ベースシステムのセンシングデータ、ベースシステム出力値、及び補正システムセンサデータは、融合フィルタに与えられる。ここで、融合フィルタには、同じタイムスタンプが関連付けられているそのようなデータ及び値のみが与えられる。時間的に対応した、ベースシステムセンサデータ、ベースシステム出力値、及び補正システムセンサデータに基づいて、補正値と補正増分（correction increments）が算出される。補正増分は、処理アルゴリズムによりベースシステムセンサデータを処理している間の、ベースシステムセンサデータの積分及び又は合算に起因する、ベースシステム出力値における誤差の、時間に伴う変化（時間変化）を反映している。次に、補正値は、対応するベースシステムセンサデータに適用され、補正されたベースシステムセンサデータに基づいて、処理アルゴリズムが、対応するベースシステム出力値を再計算する。他方で、補正増分は、対応するベースシステム出力値に適用される。すなわち、補正増分は、誤差が一定（固定、constant）のものとして見なされ得ないように、誤差の時間変動性、すなわち、例えば積分又は合算を用いて算出されたそれらのベースシステム出力値の時間依存性を処理する。再計算され補正された処理アルゴリズムへの入力値と、補正増分による算出結果の補正と、に基づいて導出される、これらの新たに算出されたベースシステム出力値は、その後、保存される。また、再計算され保存されたベースシステム出力値は、当該再計算されたベースシステム出力値の計算の基礎となったベースシステムセンサデータが最初に検知された時刻を示すタイムスタンプに関連付けられる。最後に、処理アルゴリズムの最新の出力値が、例えばナビゲーション処理などの、更なる処理に向けて出力される。

本発明の主な利点は、一定補正値を用いたベースシステム出力値の補正に加えて、補正増分が算出されることである。補正増分は、算出された後、既に保存されていたが今は再計算されている全てのベースシステム出力値の補正に用いられ、時間経過に対して一定（固定）でない追加の補正が実行される。上述した先行技術の方法とは異なり、遅延時間の間は変化しないと見なされ、従って保存されたベースシステム出力値によりカバーされる時間期間に亘っては変化しないと見なされる補正値を用いて、ベースシステム出力値のみならず、ベースシステムセンサデータも補正される。これらの補正されたベースシステムセンサデータに基づいて、次に、ベースシステム出力値の再計算を簡単に行うことができる。ベースシステムにおいて用いられる処理アルゴリズムは高速であり、したがって、リアルタイム用途の場合でも再計算を実行することができる。

他方で、補正システムは、補正システムセンサデータを、時間的に対応するベースシステム出力値と時間的に対応するベースシステムセンサデータと共に取得した後、単に補正値と補正増分とを算出する。すなわち、融合フィルタにより実行される低速の補正アルゴリズムは、過去からのデータに関して作用するが、他方で、ベースナビゲーションシステムは、常に現在のデータに関して動作する。補正は、新しい補正ループが開始したときには常に最良の利用可能なデータが使用されるように、すなわち、システム全体の正確さが高まり得るように、保存されているベースシステム出力値を再計算する際に、常に最新の補正値を用いる。ベースナビゲーションシステム出力データについては、特定の時刻に属するベースシステム出力値（複数）の補正が、異なる時刻（複数）についての補正増分（複数）により個別に補正されるため、これも可能である。すなわち、保存された全てのベースナビゲーション出力値は、それらが異なる時刻について算出された場合でも、同じ正確さを有している。

提案された方法及びシステムは、無制限の（広く一般の）マルチセンサ環境について、誤差補正を行うことができる。ベース測定システムセンサデータとベース測定システム出力値は、対応する補正測定が利用可能となるまで保存されるので、非同期且つ不規則な測定となり得る。ベース測定出力値の再計算は、上述したようにベース測定システム内でのみ実行され、新たに利用可能な補正センサデータ毎に補正システム側においも実行されるのではないので、本方法は、リアルタイムシステムにおいて容易に用いられ得る。アルゴリズムの計算負荷は、新しい補正測定が利用可能になるごとに補正値を再計算するアプローチに比べて低い。最後に、上述したように、主たる利点の一つは、誤差の時間変化も正しい方法で補正されることである。

補正システムセンサデータをそれらの対応するタイムスタンプと関連付けて保存することにより、補正システムの測定値が補正されると、さらに利点がある。すなわち、新しい補正システムセンサデータが提供される毎にではなく補正値と補正増分を算出することができ、特に、複数の補正システムセンサデータが時間に従ってソートされ得る。すなわち、例えば、特定のセンサ遅延に起因して、後の時刻に利用可能となるデータが実際にはより速い時刻に検知されていたものである、という場合には、時刻不一致（time inconsistencies）が回避される。データは、シーケンス利用可能性（sequence availability）に従ってではなく、センサデータの発生順で処理され得る。

さらに、補正システムセンサデータを破棄するための所定の閾値を定めることが好ましい。すなわち、この予め定められた所定の閾値未満の遅延時間を持つ補正システムセンサデータのみが、融合フィルタで用いられる。これにより、補正システムが現在時刻の近くで動作すること、すなわち、データが遅延してのみ利用可能である場合にはそのデータは用いられないこと、が保証される。さらに、最新の補正値が保存されるので、それらを後の時刻で利用することができる。これには、特に、予め定めた所定の閾値より短い遅延時間を持つ補正システムセンサデータが現在は何も利用できない場合には、保存されている補正値を用いた補正を実行することもできる、という利点がある。この場合には、保存されている補正値に基づいて、尚も補正が実行され得る。ただし、これでも、少なくとも一定誤差を考慮した、ベースナビゲーションシステム出力の補正が実現される。

最大遅延時間が定義され、補正値と補正増分の算出が、その最大遅延時間が経過した後にのみ実行されると、有利である。これには、補正システムセンサデータのどれが利用可能であるかが分かった時刻にのみ、融合フィルタが補正値と補正増分を算出する、という利点がある。上述したように、いくつかの補正システムセンサデータが遅延して到着することによる時刻不一致（time inconsistency）は回避され得る。

本方法は、特に、ナビゲーションシステムに適用され、従って、処理アルゴリズムは、そのようなベースナビゲーションシステムのストラップダウン・アルゴリズムである。融合フィルタは、特に、拡張誤差状態空間カルマンフィルタ（an extended error state space Kalman-filter）である。

本発明は、さらに、上述の方法を適用するよう適合された統合航法システムに関し、ベースシステムは、慣性測定ユニット、及びストラップダウン・アルゴリズムと融合フィルタとを実行するための処理ユニットを備える。

特に、補正システムは、ＧＮＳＳ、オドメトリ、気圧計（barometer）、磁気計（magnetometer）、レーダ、ステレオビジョン、レーザスキャナ、レンジファインダ（rangefinders）、ランドマーク（landmarks）、無線測位装置（radio location）の、少なくとも一つを備える。

本発明の一実施形態としての統合航法システムのコンポーネントを例示したブロックダイアグラムである。 本発明の方法ステップと、特にそれらの時間関係を例示した図である。

図１には、本発明の実施形態の一例であるナビゲーションシステム１の主要な機能ブロックを例示したブロックダイアグラムが示されている。ナビゲーションシステム１の全体は、ベースシステム２と、補正システム３と、で構成されている。ベースシステム２は、三次元の加速度及び角速度を測定する三次元加速度計及びジャイロスコープなどの、ベースセンサ４を備える。ベースシステムセンサデータは、もちろん、ベースセンサ４のそれぞれに関連する誤差（例えばセンサオフセット）を含んでいる。

ベースセンサ４により出力されるセンサデータは、処理アルゴリズムユニット６、例えばストラップダウン・アルゴリズムに与えられる。ストラップダウン・アルゴリズムにより、ベースセンサデータから出力値が算出される。これらの出力値は、例えば、速度、位置、及び姿勢に関する値を含む。ＢＮＳ出力値の少なくともいくつかについてベースシステムセンサデータを処理することにより、センサデータが積分される。これにより、もちろん、対応する出力値の誤差が時間的に変化することとなり、出力データの不正確さが時間とともに増加していくこととなる。

補正システム３は、補正システムセンサ８を備える。これらの補正システムセンサ８は、出力データを補正するためのＧＰＳ測定の場合には、衛星と受信アンテナとの間の測定された距離を含むＧＰＳ生データ測定値を出力する。もちろん、補正システムセンサデータを取得するための他のソースも可能である。

ベースシステムセンサ４により出力されるベースシステムセンサデータは、検知の時刻を示すタイムスタンプを含んでいる。ベースシステムセンサデータは、第１メモリ５に保存される。或る量のベースシステムセンサデータだけが第１メモリ５に保存されると有利である。第１メモリ５にデータが保存される時間間隔は、例えば最大遅延時間に依存する。最大遅延時間については後述において詳しく説明する。処理ユニット６内のこのストラップダウン・アルゴリズムによりベースナビゲーションセンサデータを処理することにより生成されるベースナビゲーションシステム出力値も、第２メモリ７に保存される。再び、出力値は、タイムスタンプと関連付けられる。この場合のタイムスタンプは、出力値の算出時刻を示すものではなく、その出力値の算出の基礎となった検知（sensing）に対応する時刻を示している。なお、入力されたベースナビゲーションセンサデータの処理に必要な計算コストは低く、処理アルゴリズムへのデータ入力と出力値の出力との間の時間遅延は、無視し得る。

さらに、補正システムセンサデータにもタイムスタンプが関連付けられる。これらのタイムスタンプも、対応する検知（sensing）が実行されたときの時刻を示している。補正システムセンサデータは、対応するタイムスタンプに関連付けられて、第３メモリ９に保存される。

補正アルゴリズムユニット１０は、拡張誤差状態空間カルマンフィルタなどの融合フィルタにより構成される。そのような拡張誤差状態空間カルマンフィルタ自体は、従来技術において既に公知であり、入力データから補正値を算出するのに適している。本実施形態では、補正アルゴリズムユニット１０には、第１メモリ５、第２メモリ７、及び第３メモリ９に保存されたデータが与えられる。なお、第１、第２、第３メモリ５、７、９を区別しているのは、単に例示のためである。もちろん、単一のメモリを用いて、異なる種々の値の全てを保存するものとすることができる。

補正アルゴリズムユニット１０に与えるデータや値の選択は、選択＆制御ユニット１１により制御される。ユニット１１は、対応するタイムスタンプを有する、第１メモリ５からのベースナビゲーションシステムデータ、第２メモリ７からのベースナビゲーション出力値、及び第３メモリ９からの補正センサデータを選択する。すなわち、カルマンフィルタにより実行される誤差補正計算は、時間的に対応するデータのみに基づいて実行され、これにより、出力値の時刻一致性（time consistency）が実現される。

完全に対応するデータのセットを得ることができない、ということが起こり得る。その場合には、内挿ユニット１２により内挿が実行される。すなわち、内挿ユニット１２は、補正アルゴリズムユニット１０に提供されることとなる対応するセンサデータのタイムスタンプと整合する内挿値を得ることのできる値を、第１メモリ５及び第２メモリ７から受け取る。

これにより対応することとなったベースナビゲーションセンサデータ、ベースナビゲーション出力値、及び補正システムセンサデータに基づいて、その後、補正値及び補正増分が算出される。補正増分は誤差の時間変化を補正するのに用いられ、一方、補正値は、一定値として適用される。すなわち、補正値は、変動（時間変動）の少ない誤差を補正する。そのような誤差は、例えば、センサのオフセット誤差やスケールファクタ誤差である。これに対し、補正増分は、未だ補正されていない一定誤差（constant errors）を含んだデータを処理することにより主として発生する誤差を補正する。一つの例は、加速度計データの積分を必要とする速度の算出値の補正である。そのような誤差の変化は、ストラップダウン・アルゴリズムの処理アルゴリズムから系統的（systematically）に知ることができるので、そのような補正増分の計算は、容易に行うことができる。

さらに、ストラップダウン・アルゴリズムの入力側での、残留誤差のそのような積分や合算のため、雑音などのランダム誤差も誤差全体に寄与し得ることに留意すべきである。

図２には、統合航法システムにおいて実施される場合の本発明に従う方法が例示されている。補正アルゴリズムが“過去において”実行される一方、ベースナビゲーションシステム及び従ってストラップダウン・アルゴリズムが“現在において”実行されることを示すため、タイムライン１５が示されている。用語“過去において”及び“現在において”は、以下において説明する補正システム及びベースナビゲーションシステムの方法ステップに用いられるデータの時刻を示している。すなわち、この用語は、ベースナビゲーションシステムセンサデータ、ベースナビゲーションシステム出力値、及び補正システムセンサデータのタイムスタンプに関するものである。既に説明したように、まず、ベースナビゲーションシステムのセンサ４からセンサデータが生成され、当該センサデータにタイムスタンプが与えられる。太線矢印１６で示すように、これらのセンサデータは、補正システム３において更に使用できるように保存される。ただし、ベースナビゲーションシステムセンサデータは、保存されるだけでなく、処理アルゴリズムユニット６により、そのストラップダウン・アルゴリズムにおいて、即座の処理も行われる。ストラップダウン・アルゴリズムの出力値は、外部アプリケーション（external applications）等における更なる利用のために出力されるだけでなく、黒線矢印２５で示すように、後で補正システム３で利用できるように保存される。ここで、データの処理というときは、そのような処理が複数又は一つのＣＰＵで構成され得る処理ユニット内で実行されることであることは、言うまでもない。

さらに、間隔（interval）１７で示される最大遅延時間が定義される。ベースナビゲーションセンサ４により生成された新しいデータは第１メモリ５に追加され、これにより、最大遅延時間を持つ上記間隔の間はいつでも、ベースナビゲーションシステムセンサ４の任意のセンサデータを使用したり内挿したりすることができる。最大遅延時間の間隔１７の間は、それぞれのセンサ８からの補正システムセンサデータも使用することができる。補正システムセンサは、また、検知時刻に対応するタイムスタンプと共にデータを提供する。補正システムセンサデータは、また、補正システム３に対して利用可能になるとすぐに保存される。矢印１８、１９により、これらのデータが単に後の時刻において更なる処理に利用可能となることが示されている。図示より、補正システムセンサ８による補正センサデータの当初の検知（original sensing）とベースナビゲーションシステムセンサ４の当初の検知とが、同時に、より詳細にはタイムライン１５に従う現在時刻において、発生することが判る。しかしながら、異なる種々のデータが更なる処理に利用可能となる時期の遅れである時間遅延のため、補正アルゴリズムは、補正システムセンサデータが処理され得るようになるまで待機しなければならない。したがって、図１に示すように、ベースナビゲーションシステムセンサデータに加えて、最大遅延時間内に利用可能な補正システムセンサデータも、第３メモリ９に保存される。

最大遅延時間が経過した後に、タイムスタンプに関して対応するデータが、融合フィルタに与えられる。なお、補正センサデータが異なる時刻においてのみ利用可能となることを示すため、矢印１８、１９が用いられている。保存自体は、補正システム３内で実行され得る。

最大遅延時間が経過した後、ベースナビゲーションシステムデータ及び補正システムデータが、融合フィルタにより補正アルゴリズムを実行している処理ユニットに、与えられる。アルゴリズム内において符号２０により示すように、ベースナビゲーションセンサデータ及び補正システムセンサデータからの結果として得られる出力値の間の誤差が算出される。さらに、内挿ユニット２１は、ベースナビゲーションセンサデータのタイムスタンプと補正システムセンサデータのタイムスタンプとが正確には一致しないものであり得る場合には、時刻に関して補正センサシステムデータのタイムスタンプに正確に対応する融合フィルタに与えられるベースナビゲーションセンサデータから、内挿を行う。その後、補正アルゴリズムに対して、内挿されたデータが使用される。

補正アルゴリズム内において対応する検知時刻を持つデータを用いることで、全ての測定値が、それらの到着順ではなく、測定順に処理され得ることが保証される。これは、対応するセンサデータを、それらが処理されるまで保存しておくことにより実現され、時刻についての一貫性（時刻一貫性、time consistency）を実現する。

データは、一定誤差と時間的に変化する誤差との識別を実行するモデル２２に与えられる。そのような一定誤差又は少なくとも変動の少ない誤差の例は、センサのオフセット誤差又はスケールファクタ誤差である。このほか、上述したように、ストラップダウン・アルゴリズム内でデータを処理することにより発生する誤差もある。すなわち、未だ未補正の誤差を含んでいるデータがストラップダウン・アルゴリズムに与えられる場合には、そのような誤差が、処理アルゴリズムに依存して、合算又は積分されることとなる。ただし、ストラップダウン・アルゴリズムに用いられるアルゴリズムは既知であるので、そのような誤差も、系統的（systematically）に既知である。

これにより、一定誤差のための補正値だけでなく、単位時間当たりの及び速度などの特定のＢＮＳ出力に関する、誤差の変化を表す補正増分も、算出することができる。補正値及び補正増分は、算出後に適用される。まず、補正値が、保存されている及び過去に補正された、ベースナビゲーションセンサデータに適用される。補正されたベースナビゲーションセンサデータを用いて、次に、ユニット６における処理アルゴリズムが、再び実行される。次に、ベースナビゲーション出力値が、補正増分を用いて、従って時間的に変化する誤差を考慮して、補正される。すなわち、以前のベースナビゲーションセンサデータから算出された出力値と、その後のベースナビゲーションセンサデータから算出された出力値とが、補正増分を用いて補正される。これは、処理に起因する未補正誤差の積分を反映している。融合フィルタにより２２において実行されるモデルは、ストラップダウン・アルゴリズムの入力データの誤差が積分され又は合算された態様で、ストラップダウン・アルゴリズムの異なる出力値のそれぞれに反映される。

補正されたベースナビゲーションセンサデータは、ストラップダウン・アルゴリズムにより再計算された後に、尚も対応するタイムスタンプと共に保存され、その後の補正サイクルのために使用される。これは、破線２３により示された再計算ループにより積分される。

モデル２２により出力された一定値は、これらもメモリに保存され、その後、補正増分を算出するために使用することのできる現在の補正測定値が無い場合に、利用することが可能となる。この場合、ベースナビゲーションシステム２は、スタンドアローン・モードで動作する。

新しい補正値と新しい補正増分とが利用可能なときはいつでも、既に保存されているベースナビゲーションシステムセンサデータが使用されて、ベースナビゲーション出力値が再計算される。その後、補正された出力値が、一致したタイムスタンプと共に保存されている出力値に上書きされる。すなわち、最新の現在の測定値までの利用可能な全てのデータが、最新の結果で補正される。

このアルゴリズムは、ベースシステムの測定ステップごとに繰り返される。再計算は、補正データが使用可能であった場合に実行される。

なお、上述の例は、すべてナビゲーションシステムに関するものであるが、本発明の方法の用途をこれに制限するものではない。本方法は、最大遅延時間を規定する最も低速のセンサの遅延時間内においては無視し得る変動を持つものと推定される特性を持ち、且つ他の遅延時間の際にそのような特性の誤差が蓄積されるような、すべてのマルチセンサ融合システム（multi-sensor fusion systems）に適している。これらの仮定は、誤差状態フィルタ（error-state-filters）などの誤差推定を扱うシステムにおいては明らかに成り立つ。

通常、上述したような統合航法システムにおける利用においては、ベースナビゲーションシステムは、ベースシステムとしてのＩＭＵ（慣性測定ユニット、Inertial Measurement Unit）などのセンサを用い、（ディファレンシャル）ＧＮＳＳ（全地球航法衛星システム、Global Navigation Satellite System）、オドメトリ、気圧計（barometer）、磁気計（magnetometer）、レーダ、（ステレオ）ビジョン、レーザスキャナ、レンジファインダ（rangefinders）、ランドマーク（landmarks）、無線測位装置（radio location）などにより実行される補正測定を用いる。これらは、補正センサとして用いられるだけでなく、相互に補正される。通常、そのような相互補正は、ベースナビゲーションシステムを補正するのみならずその補正測定センサをも補正する、いわゆる密結合構造（tightly coupling architectures）において行われる。上述したようなセンサは、通常、永久的には利用可能でなく、種々の異なる、非同期の、おそらくは不定のサンプリングレートを持ち、特に、様々な且つ変動する遅延時間を有している。本システムは、定義された最大遅延時間を持ち、正しい時間順に測定値を考慮するので、そのような変動する遅延時間に適合することができる。

本システムは、また、サービスロボット、例えば自動芝刈り機や、清掃の用途等に用いることができる。ただし、誤差空間及びseafareにおける用途にも、同様に適している。

Claims (8)

1. ベース測定システムＢＳと補正システムとを備える測定システムの、出力の誤差を補正する方法であって、
   ＢＳセンサデータを前記ベース測定システムから取得して、前記ＢＳセンサデータを処理アルゴリズムに与えるステップ（４）と、
   前記ＢＳセンサデータを、前記ＢＳセンサデータ検知の時刻を示すタイムスタンプに関連付けて保存するステップ（１６）と、
   前記処理アルゴリズムにおいて前記ＢＳセンサデータを処理することによりＢＳ出力値を算出するステップ（６）と、
   前記算出した前記ＢＳ出力値を、前記ＢＳ出力値の算出に用いた前記ＢＳセンサデータに関連付けられた関連する前記タイムスタンプと共に保存するステップ（７）と、
   前記補正システムのセンサデータの検知の時刻を示すタイムスタンプと関連付けられた補正システムセンサデータを前記補正システムから取得するステップ（８）と、
   対応するタイムスタンプを有する前記ＢＳセンサデータ、前記ＢＳ出力値、及び補正システムセンサデータを、融合フィルタに与えるステップと、
   前記融合フィルタによって、遅延時間中に変動のない誤差を補正するための補正値を、前記ＢＳ出力値と、対応するタイムスタンプを有する前記補正システムセンサデータとの間の誤差と、未だ補正されていない一定誤差を含む前記ＢＳセンサデータを補正するための補正増分とを算出するステップ（１０）であって、前記補正増分は、前記処理アルゴリズムにおいて前記ＢＳセンサデータを積分及び又は合算することに起因する前記ＢＳ出力値における誤差の時間変化を反映するものであるステップと、
   補正したＢＳセンサデータを生成するために対応する補正値を前記ＢＳセンサデータに適用し、前記補正したＢＳセンサデータを、前記処理アルゴリズムにより処理することにより再計算されたＢＳ出力値を生成するステップと、
   補正したＢＳ出力値を生成するために、前記補正増分を前記再計算されたＢＳ出力値に適用し、前記補正したＢＳ出力値を保存するステップと、
   前記処理アルゴリズムの最新の出力値を、更なるデータ処理のために出力するステップと、
   を有し、
   前記最新の算出された補正値は保存され、
   予め定めた所定の閾値より短い遅延時間を持つ補正システムセンサデータが何も利用できない場合には、前記保存された補正値を前記ＢＳセンサデータに適用することで補正が実行される、方法。
2. 複数の補正システムセンサデータが、当該補正システムセンサデータをそれらに対応するタイムスタンプに関連付けて記憶装置（９）に保存することにより収集される、
   ことを特徴とする、請求項１に記載の方法。
3. 前記予め定めた所定の閾値を超える遅延時間を持った補正システムセンサデータが破棄される、
   ことを特徴とする、請求項１又は２に記載の方法。
4. 前記処理アルゴリズムは、ベースナビゲーションシステム（２）におけるストラップダウン・アルゴリズムである、
   ことを特徴とする、請求項１ないし３のいずれか一項に記載の方法。
5. 前記融合フィルタは、拡張カルマンフィルタである、
   ことを特徴とする、請求項１ないし４のいずれか一項に記載の方法。
6. ベース・ナビゲーションシステム（２）と補正システム（３）とを備える統合航法システムであって、前記ベース・ナビゲーションシステム（２）及び前記補正システム（３）は、請求項１ないし５のいずれか一項に記載の方法ステップを実行するよう適合されている、
   統合航法システム。
7. 前記ベース・ナビゲーションシステム（２）は、慣性測定ユニットと、ストラップダウン・アルゴリズムを実行する処理ユニット（６）と、前記融合フィルタと、を備える、
   ことを特徴とする、請求項６に記載の統合航法システム。
8. 前記補正システム（３）は、ＧＮＳＳ、オドメトリ、気圧計、磁気計、レーダ、ステレオビジョン、レーザスキャナ、超音波レンジファインダ、ランドマーク、無線測位装置の、少なくとも一つを備える、
   ことを特徴とする、請求項６又は７に記載の統合航法システム。

Images