JP5379109B2 - ヘディングを決定するためのシステムおよび方法 - Google Patents

Description

（技術分野）
本発明は、一般的にナビゲーションシステムに関する。より詳細には、本発明は、ヘディングの決定のためのシステムおよび方法に関する。

グローバルポジショニングシステム（ＧＰＳ）の利用をもってしても、低品位の慣性計測ユニット（ＩＭＵ）は、ビークルが顕著な速度変化を時々刻々と経験しない限り、そのヘディング角度を精度良く決定できない。例えば、速度変化無しでは、ＧＰＳにより支援される１度／時間のジャイロを装備したＩＭＵのヘディング精度は約０．１ラジアンである。低品位ＩＭＵ装備をＧＰＳまたは他の外的な位置／速度の照会と整合させるための伝統的な手法は、ヘディング誤差の可観測性を提供するために、運行の期間中にＳ字ターンを利用することである。伝統的な飛行中の整合手順は、ビークルが、数分間続く長たらしい水平面Ｓ字ターン操縦を実行することを要求する。ミリラジアンの整合精度の達成が可能であるが、長たらしい伝統的な整合手順は一般的に与えられたミッションのゴールからそれる。
従来、ステレオベースの視覚的なオドメトリのための方法およびシステムがあります（例えば、特許文献１を参照）。また、従来、正確なビジョン支援ナビゲーションのための統一されたフレームワークがあります（例えば、特許文献２を参照）。また、従来、環境の拡張された状況認識および視覚化のためのシステムおよび方法があります（例えば、特許文献３を参照）。また、従来、ナビゲーション装置があります（例えば、特許文献４を参照）。また、従来、ビジョン支援カメラ位置決定があります（例えば、特許文献５を参照）。また、従来、位置決定のためのシステムおよび方法があります（例えば、特許文献６を参照）。

米国特許出願公開第２００８／０１４４９２５号明細書 米国特許出願公開第２００８／０１６７８１４号明細書 米国特許出願公開第２００７／００７００６９号明細書 米国特許第６，４１１，８９８号明細書 国際公開第００／３４８０３号 欧州特許出願公開第１９７２８９３号明細書

本発明の一実施形態において、ヘディングを決定するために、運行ビークル上に搭載可能なシステムが提供される。当該システムは、運行ビークルの運行期間中に、関心地域において、第１の時点および第２の時点で複数の識別特徴をキャプチャする画像システムを含み、第１の時点でキャプチャされた複数の識別特徴と第２の時点でキャプチャされた複数の識別特徴とを照合し、複数の照合された識別特徴のそれぞれについて、第１の時点に基く、所与の照合された識別特徴に関連する第１の単位ベクトルと、第２の時点に関連し、該所与の照合された識別特徴に関連する第２の単位ベクトルとを決定する。当該システムは、第１の時点から第２の時点までキャプチャされるキャリア位相情報に基いて並進ベクトルを決定するグローバルポジショニングシステム（ＧＰＳ）と、補正されたヘディングを決定するために、複数の照合された識別特徴のそれぞれについてエピポーラ方程式における誤差を、それぞれの第１の単位ベクトルと第２の単位ベクトルと並進ベクトルとに基いて最小化する結合プロセッサとを、さらに含む。

本発明の別の実施形態においては、ヘディングを決定するために、運行ビークル上に搭載可能なシステムが提供される。当該システムは、運行ビークルの運行期間中に、関心地域において、第１の時点および第２の時点で複数の識別特徴をキャプチャする画像システムを含み、第１の時点でキャプチャされた複数の識別特徴と第２の時点でキャプチャされた複数の識別特徴とを照合し、複数の照合された識別特徴のそれぞれについて、第１の時点に基く、所与の照合された識別特徴に関連する第１の単位ベクトルと、第２の時点に関連し、該所与の照合された識別特徴に関連する第２の単位ベクトルとを決定する。当該システムは、第１の時点から第２の時点までキャプチャされるキャリア位相情報に基いて並進ベクトルを決定するグローバルポジショニングシステム（ＧＰＳ）と、補正されたヘディングを決定するために、複数の照合された識別特徴のそれぞれについてエピポーラ方程式における誤差を、それぞれの第１の単位ベクトルと第２の単位ベクトルと並進ベクトルとに基いて最小化する結合プロセッサとを、さらに含む。当該システムはまた、結合プロセッサに初期のヘディングを提供する慣性計測システムを含み、それによって、結合プロセッサがエピポーラ方程式を最小化するために開始点で初期ヘディングを利用する。

本発明のまたさらなる実施形態において、画像システムと慣性システムとグローバルポジショニングシステム（ＧＰＳ）とを利用する運行ビークルのヘディングを決定する方法が提供される。当該方法は、一の関心地域において影像をキャプチャし、第１の時点で複数の識別特徴の座標を抽出することと、該一の関心地域において影像をキャプチャし、第２の時点で複数の識別特徴の座標を抽出することと、複数の照合された識別特徴を決定するために第１の時点でキャプチャされた複数の識別特徴の座標と第２の時点でキャプチャされた複数の識別特徴の座標とを照合することとを含む。当該方法は、複数の照合された識別特徴のそれぞれについて、第１の時点に基く、所与の照合された識別特徴に関連する第１の単位ベクトル、および第２の時点に関連し、該所与の照合された識別特徴に関連する第２の単位ベクトルを提供することと、第１の時点から第２の時点までキャプチャされるキャリア位相情報に基いて並進ベクトルを計算することと、補正されたヘディングを決定するために第１の単位ベクトルと第２の単位ベクトルと並進ベクトルとに基いて、複数の照合された識別特徴のそれぞれについてエピポーラ方程式を最小化することとを、さらに含む。

以上により、本発明は、以下の手段を提供する。

（項目１）
運行ビークル上に搭載可能なヘディングを決定するためのシステムであって、該システムは、
画像システムであって、該画像システムは、該運行ビークルの運行期間中に関心地域において第１の時点および第２の時点で複数の識別特徴をキャプチャし、該第１の時点でキャプチャされた該複数の識別特徴と該第２の時点でキャプチャされた該複数の識別特徴とを照合し、該複数の照合された識別特徴のそれぞれについて、該第１の時点に基く、所与の照合された識別特徴に関連する第１の単位ベクトルと、該第２の時点に関連し、該所与の照合された識別特徴に関連する第２の単位ベクトルとを決定する、画像システムと、
該第１の時点から該第２の時点までキャプチャされるキャリア位相情報に基いて並進ベクトルを決定するグローバルポジショニングシステム（ＧＰＳ）と、
結合プロセッサであって、補正されたヘディングを決定するために、該複数の照合された識別特徴のそれぞれについてエピポーラ方程式における誤差を、それぞれの該第１の単位ベクトルと該第２の単位ベクトルと該並進ベクトルとに基いて最小化する、結合プロセッサと
を備えている、システム。

（項目２）
上記結合プロセッサに初期レベリングを提供する慣性計測システムをさらに備え、該結合プロセッサはヘディングについて上記エピポーラ方程式を直接的に解くために該初期レベリングを利用する、上記項目に記載のシステム。

（項目３）
上記慣性計測システムは、慣性計測ユニット（ＩＭＵ）と慣性プロセッサとを備え、
該ＩＭＵは、速度変化および姿勢変化を該慣性プロセッサに提供し、
該慣性プロセッサは、位置、ヘディングおよび姿勢のそれぞれの評価値を決定する、
上記項目のうちのいずれか１項に記載のシステム。

（項目４）
上記エピポーラ方程式は（ｘ_１ ^Ｌ×ｘ_２ ^Ｌ）・Ｃ_Ｎ ^ＬＴ^Ｎ＝０であり、ここで、ｘ_１ ^Ｌは上記第１の単位ベクトル、ｘ_２ ^Ｌは上記第２の単位ベクトル、Ｔ^Ｎは上記並進ベクトル、およびＣ_Ｎ ^Ｌは下記のようなナビゲーションフレーム（Ｎ）フレームから局所フレーム（Ｌ）フレームへの変換マトリクスであり、

ここで、ｃ＝ｃｏｓ（ａ）およびｓ＝ｓｉｎ（ａ）であり、ａは解かれるべきヘディング角度である、上記項目のうちのいずれか１項に記載のシステム。

（項目５）
上記結合プロセッサに初期ヘディングを提供する慣性計測システムをさらに備え、
該結合プロセッサは、開始点で該初期ヘディングを利用し、上記エピポーラ方程式における誤差を最小化するヘディング角度が提供されるまで該ヘディング角度を繰り返し補正する、
上記項目のうちのいずれか１項に記載のシステム。

（項目６）
上記結合プロセッサは、上記システムに対するヘディングを直接的に計算するために、上記複数の照合された識別特徴のそれぞれの上記決定された補正されたヘディングを平均化する、上記項目のうちのいずれか１項に記載のシステム。

（項目７）
上記結合プロセッサはカルマンフィルタであり、上記エピポーラ方程式はδｒ＝Ｔ×（ｘ_１×ｘ_２）・φ＋（ｘ_１×ｘ_２）・δＴであり、ここで、ｘ_１は上記第１の単位ベクトル、ｘ_２は上記第２の単位ベクトル、Ｔは上記並進ベクトルであり、ここで、φ＝姿勢誤差およびδＴ＝並進誤差であり、該カルマンフィルタが姿勢誤差φの解を得、δＴは上記ＧＰＳによって計測される、上記項目のうちのいずれか１項に記載のシステム。

（項目８）
上記ＧＰＳは、ＧＰＳデバイスとＧＰＳプロセッサとを備え、
該ＧＰＳデバイスは、キャリア位相情報を提供し、
該ＧＰＳプロセッサは、上記並進ベクトルを決定する、
上記項目のうちのいずれか１項に記載のシステム。

（項目９）
上記画像システムは、画像センサと画像プロセッサとを備え、
該画像センサは、上記運行ビークルの運行期間中に上記関心地域において上記第１の時点および上記第２の時点で上記複数の識別特徴をキャプチャし、
該画像プロセッサは、該第１の時点でキャプチャされた該複数の識別特徴と該第２の時点でキャプチャされた該複数の識別特徴とを照合し、該複数の照合された識別特徴のそれぞれについて、該第１の時点に基く、所与の照合された識別特徴に関連する上記第１の単位ベクトルと、該第２の時点に関連し、該所与の照合された識別特徴に関連する上記第２の単位ベクトルとを決定する、
上記項目のうちのいずれか１項に記載のシステム。

（項目１０）
運行ビークル上に搭載可能なヘディングを決定するためのシステムであって、該システムは、
画像システムであって、該画像システムは、該運行ビークルの運行期間中に関心地域において第１の時点および第２の時点で複数の識別特徴をキャプチャし、該第１の時点でキャプチャされた該複数の識別特徴と該第２の時点でキャプチャされた該複数の識別特徴とを照合し、該複数の照合された識別特徴のそれぞれについて、該第１の時点に基く、所与の照合された識別特徴に関連する第１の単位ベクトルと、該第２の時点に関連し、該所与の照合された識別特徴に関連する第２の単位ベクトルとを決定する、画像システムと、
該第１の時点から該第２の時点までキャプチャされるキャリア位相情報に基いて並進ベクトルを決定するグローバルポジショニングシステム（ＧＰＳ）と、
結合プロセッサであって、補正されたヘディングを決定するために、該複数の照合された識別特徴のそれぞれについてエピポーラ方程式における誤差を、それぞれの該第１の単位ベクトルと該第２の単位ベクトルと該並進ベクトルとに基いて最小化する、結合プロセッサと、
該結合プロセッサに初期ヘディングを提供する慣性計測システムであって、該結合プロセッサは、該エピポーラ方程式を最小化するために開始点で該初期ヘディングを利用する、慣性計測システムと
を備えている、システム。

（項目１１）
上記エピポーラ方程式は（ｘ_１ ^Ｌ×ｘ_２ ^Ｌ）・Ｃ_Ｎ ^ＬＴ^Ｎ＝０であり、ここで、ｘ_１ ^Ｌは上記第１の単位ベクトル、ｘ_２ ^Ｌは上記第２の単位ベクトル、Ｔ^Ｎは上記並進ベクトル、およびＣ_Ｎ ^Ｌは下記のようなナビゲーションフレーム（Ｎ）フレームから局所フレーム（Ｌ）フレームへの変換マトリクスであり、

ここで、ｃ＝ｃｏｓ（ａ）およびｓ＝ｓｉｎ（ａ）であり、ａは解かれるべきヘディング角度である、上記項目のうちのいずれか１項に記載のシステム。

（項目１２）
上記結合プロセッサは、上記システムに対する上記ヘディングを直接的に計算するために、上記複数の照合された識別特徴のそれぞれの上記決定された補正されたヘディングを平均化する、上記項目のうちのいずれか１項に記載のシステム。

（項目１３）
上記結合プロセッサはカルマンフィルタであり、上記エピポーラ方程式はδｒ＝Ｔ×（ｘ_１×ｘ_２）・φ＋（ｘ_１×ｘ_２）・δＴであり、ここで、ｘ_１は上記第１の単位ベクトル、ｘ_２は上記第２の単位ベクトル、Ｔは上記並進ベクトルであり、ここで、φ＝姿勢誤差およびδＴ＝並進誤差であり、該カルマンフィルタが姿勢誤差φの解を得、δＴはＧＰＳによって計測される、上記項目のうちのいずれか１項に記載のシステム。

（項目１４）
上記ＧＰＳは、ＧＰＳデバイスとＧＰＳプロセッサとを備え、
該ＧＰＳデバイスは、キャリア位相情報を提供し、
該ＧＰＳプロセッサは、上記並進ベクトルを決定し、
上記画像システムは、画像センサと画像プロセッサとを備え、
該画像センサは、上記運行ビークルの運行期間中に上記関心地域において上記第１の時点および上記第２の時点で上記複数の識別特徴をキャプチャし、
該画像プロセッサは、該第１の時点でキャプチャされた該複数の識別特徴と該第２の時点でキャプチャされた該複数の識別特徴とを照合し、該複数の照合された識別特徴のそれぞれについて、該第１の時点に基く、所与の照合された識別特徴に関連する上記第１の単位ベクトルと、該第２の時点に関連し、該所与の照合された識別特徴に関連する上記第２の単位ベクトルとを決定し、
上記慣性計測システムは、慣性計測ユニット（ＩＭＵ）と慣性プロセッサとを備え、
該ＩＭＵは、速度変化および姿勢変化を該慣性プロセッサに提供し、
該慣性プロセッサは、位置、ヘディングおよび姿勢のそれぞれの評価値を決定する、
上記項目のうちのいずれか１項に記載のシステム。

（項目１５）
画像システムと慣性システムとグローバルポジショニングシステム（ＧＰＳ）とを利用する運行ビークルのヘディングを決定する方法であって、該方法は、
一の関心地域において影像をキャプチャし、第１の時点で複数の識別特徴の座標を抽出することと、
該一の関心地域において影像をキャプチャし、第２の時点で複数の識別特徴の座標を抽出することと、
複数の照合された識別特徴を決定するために、該第１の時点でキャプチャされた該複数の識別特徴の座標と該第２の時点でキャプチャされた該複数の識別特徴の座標とを照合することと、
該複数の照合された識別特徴のそれぞれについて、該第１の時点に基く、所与の照合された識別特徴に関連する第１の単位ベクトル、および該第２の時点に関連し、該所与の照合された識別特徴に関連する第２の単位ベクトルを提供することと、
該第１の時点から該第２の時点までキャプチャされるキャリア位相情報に基いて、並進ベクトルを計算することと、
補正されたヘディングを決定するために、該第１の単位ベクトルと該第２の単位ベクトルと該並進ベクトルとに基いて、該複数の照合された識別特徴のそれぞれについてエピポーラ方程式を最小化することと
を包含している、方法。

（項目１６）
上記エピポーラ方程式を最小化するために開始点で上記ＩＭＵによって提供される初期ヘディングを利用することをさらに包含している、上記項目のうちのいずれか１項に記載の方法。

（項目１７）
上記エピポーラ方程式は（ｘ_１ ^Ｌ×ｘ_２ ^Ｌ）・Ｃ_Ｎ ^ＬＴ^Ｎ＝０であり、ここで、ｘ_１ ^Ｌは上記第１の単位ベクトル、ｘ_２ ^Ｌは上記第２の単位ベクトル、Ｔ^Ｎは上記並進ベクトル、およびＣ_Ｎ ^Ｌは下記のようなナビゲーションフレーム（Ｎ）フレームから局所フレーム（Ｌ）フレームへの変換マトリクスであり、

ここで、ｃ＝ｃｏｓ（ａ）およびｓ＝ｓｉｎ（ａ）であり、ａは解かれるべきヘディング角度である、上記項目のうちのいずれか１項に記載の方法。

（項目１８）
上記整合されたヘディングを直接的に計算するために、上記複数の照合された識別特徴のそれぞれの上記決定された補正されたヘディングを平均化することをさらに包含している、上記項目のうちのいずれか１項に記載の方法。

（項目１９）
上記エピポーラ方程式はδｒ＝Ｔ×（ｘ_１×ｘ_２）・φ＋（ｘ_１×ｘ_２）・δＴであり、ここで、ｘ_１は上記第１の単位ベクトル、ｘ_２は上記第２の単位ベクトル、Ｔは上記並進ベクトルであり、ここで、φ＝姿勢誤差およびδＴ＝並進誤差である、上記項目のうちのいずれか１項に記載の方法。

（項目２０）
カルマンフィルタが上記姿勢誤差φの解を得、上記δＴは上記ＧＰＳによって計測される、上記項目のうちのいずれか１項に記載の方法。

（摘要）
運行ビークルのヘディングを決定するためのシステムおよび方法が提供される。当該システムおよび方法は、第１および第２の時点で画像エリアの複数の識別情報をキャプチャおよび照合し、複数の識別特徴に関連する単位ベクトルを決定するために、画像システムを利用する。グローバルポジショニングシステム（ＧＰＳ）は、第１および第２の時点について並進ベクトルを提供し、結合プロセッサは、補正されたヘディングを提供するために、単位ベクトルおよび並進ベクトルを利用する。

図１は、本発明の一局面に従ってヘディングを決定するためのシステムのブロックダイヤグラムを図示する。 図２は、本発明の一局面に従って結合プロセッサにより遂行される計算の説明を容易にするエピポーラ幾何グラフを図示する。 図３は、第１のシミュレーションにおける慣性姿勢誤差対時間のグラフを図示し、そのような第１のシミュレーションは、ＧＰＳを有さず、Ｓ字ターン整合操縦を有した、気圧支援ナビゲーション品位の慣性ナビゲーションシステム（ＩＮＳ）の自由慣性姿勢誤差を有して遂行された。 図４は、第２のシミュレーションにおける慣性姿勢誤差対時間のグラフを図示し、そのような第２のシミュレーションは、Ｓ字ターン整合を利用する気圧支援ナビゲーション品位のＩＮＳを有さず、本発明の一局面に従って連続的な整合用の映像観測を有して遂行された。 図５は、本発明の一局面に従ってヘディングを決定するための方法論を図示する。

（詳細な説明）
映像的な手掛かりを使用したヘディングを決定するためのシステムおよび方法が提供される。映像的な手掛かりの使用は、Ｓ字ターン操縦の必要性を削減する一方で同様の性能を提供し、高品質な機器類が使用される場合にヘディング精度を改善する可能性を提示する。本発明の一実施形態において、ヘディングを決定するために、運行ビークル上に搭載可能なシステムが提供される。当該システムは、運行ビークルの運動期間中に、関心地域において、継続的な時点で複数の識別特徴を認定および追跡する画像システムを含む。映像処理手順は、複数の特徴を照合することと、それから、これらの照合された識別特徴のそれぞれに関連する単位ベクトルを決定することとを含む。当該システムは、第１の時点から第２の時点までキャプチャされるキャリア位相情報に基いて並進ベクトルを決定するグローバルポジショニングシステム（ＧＰＳ）と、複数の照合された識別特徴のそれぞれについてエピポーラ方程式における誤差を、それぞれの第１の単位ベクトルと第２の単位ベクトルと並進ベクトルとに基いて最小化する結合プロセッサとを、さらに含む。カルマンフィルタ（または他の最適な評価手段）は、精練されたヘディングの解決手段のために計測を連続的に結合するのに利用され得る。

プラットフォームの整合は２つのステップを要求することが理解されるべきである。すなわち、任意の地球固定フレームとボディフレームとの間の関係を決定すること、それから既知の地球固定フレーム（例えば、北、東、下）に対するこの地球固定フレームを決定することである。十分に照合された特徴により、映像観測は地球固定座標フレーム（北、東、および下への関係は未だ未知）において参照されるプラットフォームのポーズへと結合され得る。それから結合プロセッサは、ボディフレームと既知の地球固定フレームとの間の最終的な整合を決定するために、映像観測をＧＰＳ計測と結合し得る。

本発明の別の実施形態においては、結合プロセッサは、開始点で初期ヘディングについて直接的に解くために、エピポーラ方程式（式３）と共にＧＰＳ並進および映像観測を利用する。あるいは、慣性計測システムは結合プロセッサに開始点で初期ヘディングを提供し得る。

これ以後、例示の簡便のため、特徴照合の記載は逐次的な２つの時点の間の照合として記載されるが、この操作は２つ以上の時点を含み得る。

図１は、本発明の一局面に従ってヘディングを決定するためのシステム１０を図示する。システム１０は、運行ビークル（示されていない）上に搭載される、慣性計測システム１２、画像システム１８およびグローバルポジションシステム（ＧＰＳ）２４を含む。慣性計測システム１２は、比較的低廉な慣性計測ユニット（ＩＭＵ）１４および慣性プロセッサ１６を含む。画像システム１８は、画像センサ２０（例えばカメラ）および画像プロセッサ２２を含む。ＧＰＳ２４は、ＧＰＳ受信器２６およびＧＰＳプロセッサ２８を含む。一の画像エリアの複数の識別特徴は、運行ビークルの運動期間中に、第１の時点（ｔ_１）および第２の時点（ｔ_２）で画像センサ２０によってキャプチャされる。複数の識別特徴の照合後、画像プロセッサ２２によって、複数の照合された特徴のそれぞれについて、第１の単位ベクトル（ｘ_１）は第１の時点（ｔ_１）で決定され得、第２の単位ベクトル（ｘ_２）は第２の時点（ｔ_２）で決定され得る。画像プロセッサ２２は、複数の特徴のそれぞれについて、第１の単位ベクトル（ｘ_１）および第２の単位ベクトル（ｘ_２）を結合プロセッサ３０に提供する。並進ベクトルＴ_ＧＰＳは、第１の時点（ｔ_１）から第２の時点（ｔ_２）までのＧＰＳ受信器２６によってキャプチャされるキャリア位相情報に基いて、ＧＰＳプロセッサ２８によって決定され得る。ＧＰＳプロセッサ２８は、並進ベクトルＴ_ＧＰＳを結合プロセッサ３０に提供する。

初期整合の期間中、ＧＰＳプロセッサ２８からの並進情報と並行して、速度の変化（ΔＶ）および姿勢の変化（Δθ）がＩＭＵ１４から慣性プロセッサ１６に提供され、これにより、結合プロセッサ３０は初期位置、初期姿勢および初期ヘディングを決定できる。ＩＭＵ１４は速度および姿勢の変化を提供し続け、慣性プロセッサ１６は第１の時点（ｔ_１）と第２の時点（ｔ_２）との間における位置、姿勢およびヘディングを伝達する。これらの値は結合プロセッサ３０に提供され、結合プロセッサ３０はまた慣性プロセッサ１６に補正情報をフィードバックする。

並進ベクトルＴ_ＧＰＳは、ナビゲーションフレームＮ（北−東−下）においてＧＰＳプロセッサ２８によって決定され、第１の単位ベクトルｘ_１および第２の単位ベクトルｘ_２は、ＩＭＵ１４のボディフレームＢに堅固に関係した画像プロセッサ２２によって決定される。結合プロセッサ３０は、エピポーラ方程式における誤差を解き、および最小化するように構成され、
（ｘ_１×ｘ_２）・Ｔ＝０ 式１
ここで、ｘ_１およびｘ_２は、それぞれ、点ｏ_１および点ｏ_２から同一の特徴を指す第１の単位ベクトルおよび第２の単位ベクトルである。便宜（および照準誤差を無視）のため、画像フレームとシステムボディフレームとは同一であると仮定する。また、ＩＭＵ１４のロール角度およびピッチ角度は既知（それらは重力ベクトルの計測によって直ちに決定される）であると仮定する。つまりボディフレームから局地水平面フレームＬへの変換マトリクスＣ_Ｂ ^Ｌが既知であることを意味する。これにより、ヘディング角度を解くことの問題は、ナビゲーション（Ｎ）フレームから局地（Ｌ）フレームへの変換マトリクスＣ_Ｎ ^Ｌを解くことに低減される。

上述した既知の誤差を無視することに基いて、および上述した既知の仮定に基いて、結合プロセッサ３０は、下記のようなナビゲーション（Ｎ）フレームから局地（Ｌ）フレームへの変換マトリクスＣ_Ｎ ^Ｌを解くことによってヘディング角度を決定し、

ここで、ｃ＝ｃｏｓ（ａ）およびｓ＝ｓｉｎ（ａ）であり、ａは解かれるべきヘディング角度である。

初期ヘディング角度ａは、慣性プロセッサ１６から提供され得、下記のエピポーラ方程式に代入され、
（ｘ_１ ^Ｌ×ｘ_２ ^Ｌ）・Ｃ_Ｎ ^ＬＴ^Ｎ＝０ 式３
ヘディング角度は、最小化される（例えば、おおよそ０）エピポーラ方程式の結果を提供するヘディング角度が提供されるまで、結合プロセッサ３０によって繰り返し補正される。これは複数の識別特徴のそれぞれについて反復される。結合プロセッサ３０は、複数の識別特徴から導かれるヘディング角度を平均化し、システム１０に対する初期ヘディング整合を出力する。

ヘディングを決定するためのシステム１０は、たくさんの有用な用途を有する。主に、この技術は、航空機、陸上ビークル、またはヘリコプタにおいて、慣例の軌跡またはＳ字ターンのような操縦の必要性を有さずに、そのヘディングを決定するのに低コストなスタンドアロンのＧＰＳ−ＩＭＵ−カメラのパッケージの使用を可能にする。このことはまた、例えば電子／光学センサまたはＳＡＲ（合成開口レーダ）などの、ある種の映像センサが既に利用可能であるような状況にも適用され得る。

図２は、本発明の一局面に従って結合プロセッサにより遂行される計算の記載を容易にするエピポーラ幾何グラフ４０を図示する。３Ｄ空間における地点ｐでの特徴は画像センサによって２度（ｔ_１およびｔ_２）観測され、それぞれ点ｏ_１および点ｏ_２に位置付けられる。Ｘ_１およびＸ_２を、北−東−下ナビゲーション座標フレーム（Ｎ）におけるそれぞれｏ_１およびｏ_２からｐへのベクトルとする。Ｔをｏ_１からｏ_２への並進ベクトルとする。画像センサは特徴への方向（単位ベクトル）を提供する。それはＩＭＵのボディフレーム（Ｂ）に堅固に関係した画像センサフレームにおいて計測され、それからナビゲーションフレームへ変換される。便宜（単一焦点距離を仮定および照準誤差を無視）のため、画像センサフレームおよびＩＭＵボディフレームは同一であると仮定する。例えば、

とし、エピポーラ方程式は下記のように表わされ、
ｒ＝ｘ_２・（Ｔ×ｘ_１）＝０， 式５
ここで、「・」は内積であり、「×」は外積である。
上記の方程式は単に、３ベクトルｘ_１、ｘ_２、およびＴは同一面にあることを表わす。式５は下記のように書き直され得、
ｒ＝（ｘ_１×ｘ_２）・Ｔ＝０， 式６
となる。

本発明の一局面において、結合プロセッサ３０はカルマンフィルタである。上述の解決手段は、以下に記載されるようにカルマンフィルタを使用することによって精練され得る。画像センサフレームはＩＭＵフレームに関係しているから、画像計測はＩＭＵフレームと同一の姿勢誤差を共有する。式６の誤差は下記のように表現され得、
δｒ＝−φ×（ｘ_１×ｘ_２）・Ｔ＋（ｘ_１×ｘ_２）・δＴ ＋計測ノイズ 式７
δｒ＝Ｔ×（ｘ_１×ｘ_２）・φ＋（ｘ_１×ｘ_２）・δＴ ＋計測ノイズ 式８
ここで、φ＝姿勢誤差（傾斜誤差およびヘディング誤差）、δＴ＝並進誤差。
δＴはＧＰＳによって精度良く計測され得るから、式８はカルマンフィルタが姿勢誤差φ（傾斜誤差およびヘディング誤差）の解を得るのに十分な情報を提供する。

図３は、第１のシミュレーションにおける慣性姿勢誤差対時間のグラフ５０を図示し、そのような第１のシミュレーションは、Ｓ字ターン整合後、ＧＰＳを拒否した期間中に気圧支援ナビゲーション品位のＩＮＳの自由慣性姿勢誤差を有して遂行された。図４は、第２のシミュレーションにおける慣性姿勢誤差対時間のグラフ６０を図示し、そのような第２のシミュレーションは、Ｓ字ターン整合を利用する気圧支援ＩＮＳを有さず、本発明の一局面に従って画像センサを使用する連続的な整合を有して遂行された。結果を図示するために、Ｓ字ターン整合を有して映像による整合を有しない気圧支援の自由慣性姿勢の精度（３０回のモンテカルロ実行から１σ）が図３に図示されている。映像が含まれる場合にＳ字ターン操縦は取り除かれ得、性能は殆ど同一であるから、ヘディング精度は主に整合手順の恩恵である。この例では、映像支援は、逐次的なフレーム間で５つの特徴を照合する下向きに見るＥＯ映像センサによって提供された。シミュレーションされた映像精度は、０．２２ｍｒａｄの特徴ノイズおよびトータルレンジの０．１％の特徴レンジ精度から成っていた。これらのパラメータは高性能カメラの象徴となり得る。これに対応する、映像による整合を有しＳ字ターンを有しない姿勢誤差が図４に図示されている。図４は、図３のＳ字ターンの場合と同様なヘディング整合精度を、映像を使用して、任務プロファイルに（Ｓ字ターンのような）特別な操縦を追加する必要無く、図示していることに留意されるべきである。

上述された前記の構造的および機能的特徴に照らして、本発明の様々な局面に従った方法論が図５の参照と共により理解されよう。説明の簡便のために、図５の方法論は直列に実行するように示され記載されているが、本発明は図示された順序に限定されず、ある局面では、本発明に従って、異なる順序および／または本明細書中に示され記載される他の局面と共に発生し得ることを理解されるべきである。さらに言えば、本発明の一局面に従って方法論を実施するのに、すべての図示された特徴は要求されないかもしれない。

図５は、本発明の一局面に従ってヘディングを決定するための方法論７０の例を図示する。当該方法論は、画像システム、慣性計測システムおよび運行ビークル上に搭載されるＧＰＳを利用する。７２では、一の画像エリアの影像が運行ビークルの運行期間中に第１の時点（ｔ_１）で画像センサによってキャプチャされ、複数の識別特徴の座標が画像プロセッシングによって抽出される。７４では、該一の画像エリアの影像が運行ビークルの運行期間中に第２の時点（ｔ_２）で画像センサによってキャプチャされ、複数の識別特徴の座標が画像プロセッシングによって抽出される。７６では、識別特徴の座標が照合され、複数の照合された識別特徴のそれぞれについて、第１の時点（ｔ_１）に基いて第１の単位ベクトル（ｘ_１）が決定され、第２の時点（ｔ_２）に基いて第２の単位ベクトル（ｘ_１）が決定される。方法論７０はそれから７８に進む。

７８では、並進ベクトルＴ_ＧＰＳが、第１の時点（ｔ_１）から第２の時点（ｔ_２）までのＧＰＳデバイスによって提供されるキャリア位相情報に基いて計算される。８０では、慣性計測システムが結合プロセッサに初期レベリングを提供する。結合プロセッサは、式３に基いてヘディングについてエピポーラ方程式を直接的に解くために初期レベリングを利用し得る。８２では、照合された識別特徴のそれぞれに関連する補正されたヘディングを決定するために、所与の照合された識別特徴についての第１の単位ベクトル（ｘ_１）および第２の単位ベクトル（ｘ_２）と、並進ベクトルＴ_ＧＰＳと、８０からの初期レベリングとに基いて、照合された識別特徴のそれぞれについてエピポーラ方程式における誤差が最小化される。複数の補正されたヘディングは、一つの精巧に整合された補正されたヘディングを提供するために平均化され得る。エピポーラ幾何方程式は、初期ヘディングを直接的に計算する場合は式３に基き得、ＩＭＵが利用可能な場合は式８に基き得る。結合プロセッサはヘディングを連続的に精練するのにカルマンフィルタを使用し得る。

これまでの上述は本発明の例である。勿論、本発明を記載する目的で構成要素または方法論のあらゆる想定される組合せを記載することは不可能であるが、本分野における通常の技術を有する者は本発明のさらに多くの組合せおよび入替えが可能であることを認識されよう。従って、本発明はこれらの変更、改変および変形のすべてを包含し、添付の特許請求の範囲の意図および範囲内にあることを意図している。

１０ ヘディングを決定するためのシステム
１２ 慣性計測システム
１４ 慣性計測ユニット（ＩＭＵ）
１６ 慣性プロセッサ
１８ 画像システム
２０ 画像センサ
２２ 画像プロセッサ
２４ グローバルポジショニングシステム（ＧＰＳ）
２６ ＧＰＳ受信器
２８ ＧＰＳプロセッサ
３０ 結合プロセッサ
４０ エピポーラ幾何グラフ
５０ 第１のシミュレーションにおける慣性姿勢誤差対時間のグラフ
６０ 第２のシミュレーションにおける慣性姿勢誤差対時間のグラフ
７０ ヘディングを決定するための方法論

Claims (17)

1. 走行するビークル上に搭載可能なシステムであって、該システムは、該ビークルが走行する方向を表すヘディングを決定するためのシステムであり、該システムは、
   該走行するビークルの走行期間中の第１の時点および第２の時点で、関心地域における複数の識別特徴をキャプチャし、該第１の時点でキャプチャされた該複数の識別特徴と該第２の時点でキャプチャされた該複数の識別特徴とを照合し、該複数の照合された識別特徴のそれぞれについて、該第１の時点に基づく、所与の照合された識別特徴に関連する第１の単位ベクトルと、該第２の時点に関連し、該所与の照合された識別特徴に関連する第２の単位ベクトルとを決定する画像システムと、
   該第１の時点から該第２の時点までにキャプチャされたキャリア位相情報に基づいて並進ベクトルを決定するグローバルポジショニングシステム（ＧＰＳ）と、
   該第１の単位ベクトルと該第２の単位ベクトルと該並進ベクトルとにそれぞれ基づいて、該複数の照合された識別特徴のそれぞれについてエピポーラ方程式における誤差を最小化することにより、補正されたヘディングを決定する結合プロセッサと
   を備えており、
   該結合プロセッサはカルマンフィルタであり、該エピポーラ方程式はδｒ＝Ｔ×（ｘ _１ ×ｘ _２ ）・φ＋（ｘ _１ ×ｘ _２ ）・δＴであり、ここで、ｘ _１ は該第１の単位ベクトル、ｘ _２ は該第２の単位ベクトル、Ｔは該並進ベクトルであり、ここで、φ＝姿勢誤差であり、δＴ＝並進誤差であり、該カルマンフィルタが姿勢誤差φの解を得、δＴは該ＧＰＳによって計測される、システム。
2. 前記結合プロセッサに初期レベリングを提供する慣性計測システムをさらに備え、該結合プロセッサは、ヘディングについて前記エピポーラ方程式を直接的に解くために該初期レベリングを利用する、請求項１に記載のシステム。
3. 前記慣性計測システムは、慣性計測ユニット（ＩＭＵ）と慣性プロセッサとを備え、
   該ＩＭＵは、速度変化および姿勢変化を該慣性プロセッサに提供し、
   該慣性プロセッサは、位置、ヘディング、姿勢のそれぞれの評価値を決定する、請求項２に記載のシステム。
4. 前記エピポーラ方程式は（ｘ_１ ^Ｌ×ｘ_２ ^Ｌ）・Ｃ_Ｎ ^ＬＴ^Ｎ＝０であり、ここで、ｘ_１ ^Ｌは前記第１の単位ベクトル、ｘ_２ ^Ｌは前記第２の単位ベクトル、Ｔ^Ｎは前記並進ベクトル、Ｃ_Ｎ ^Ｌは下記のようなナビゲーション（Ｎ）フレームから局所（Ｌ）フレームへの変換マトリクスであり、
   ここで、ｃ＝ｃｏｓ（ａ）であり、ｓ＝ｓｉｎ（ａ）であり、ａは解かれるべきヘディング角度である、請求項１に記載のシステム。
5. 前記結合プロセッサに初期ヘディングを提供する慣性計測システムをさらに備え、
   該結合プロセッサは、開始点で該初期ヘディングを利用し、前記エピポーラ方程式における誤差を最小化するヘディング角度が提供されるまで該ヘディング角度を繰り返し補正する、請求項４に記載のシステム。
6. 前記結合プロセッサは、前記システムに対するヘディングを直接的に計算するために、前記複数の照合された識別特徴のそれぞれの前記決定された補正されたヘディングを平均化する、請求項１に記載のシステム。
7. 前記ＧＰＳは、ＧＰＳデバイスとＧＰＳプロセッサとを備え、
   該ＧＰＳデバイスは、キャリア位相情報を提供し、
   該ＧＰＳプロセッサは、前記並進ベクトルを決定する、請求項１に記載のシステム。
8. 前記画像システムは、画像センサと画像プロセッサとを備え、
   該画像センサは、前記走行するビークルの走行期間中の前記第１の時点および前記第２の時点で、前記関心地域における前記複数の識別特徴をキャプチャし、
   該画像プロセッサは、該第１の時点でキャプチャされた該複数の識別特徴と該第２の時点でキャプチャされた該複数の識別特徴とを照合し、該複数の照合された識別特徴のそれぞれについて、該第１の時点に基づく、所与の照合された識別特徴に関連する前記第１の単位ベクトルと、該第２の時点に関連し、該所与の照合された識別特徴に関連する前記第２の単位ベクトルとを決定する、請求項１に記載のシステム。
9. 走行するビークル上に搭載可能なシステムであって、該システムは、該ビークルが走行する方向を表すヘディングを決定するためのシステムであり、該システムは、
   該走行するビークルの走行期間中の第１の時点および第２の時点で、関心地域における複数の識別特徴をキャプチャし、該第１の時点でキャプチャされた該複数の識別特徴と該第２の時点でキャプチャされた該複数の識別特徴とを照合し、該複数の照合された識別特徴のそれぞれについて、該第１の時点に基づく、所与の照合された識別特徴に関連する第１の単位ベクトルと、該第２の時点に関連し、該所与の照合された識別特徴に関連する第２の単位ベクトルとを決定する画像システムと、
   該第１の時点から該第２の時点までにキャプチャされたキャリア位相情報に基づいて並進ベクトルを決定するグローバルポジショニングシステム（ＧＰＳ）と、
   該第１の単位ベクトルと該第２の単位ベクトルと該並進ベクトルとにそれぞれ基づいて、該複数の照合された識別特徴のそれぞれについてエピポーラ方程式における誤差を最小化することにより、補正されたヘディングを決定する結合プロセッサと、
   該結合プロセッサに初期ヘディングを提供する慣性計測システムであって、該結合プロセッサは、該エピポーラ方程式を最小化するために開始点で該初期ヘディングを利用する、慣性計測システムと
   を備えており、
   該結合プロセッサはカルマンフィルタであり、該エピポーラ方程式はδｒ＝Ｔ×（ｘ _１ ×ｘ _２ ）・φ＋（ｘ _１ ×ｘ _２ ）・δＴであり、ここで、ｘ _１ は該第１の単位ベクトル、ｘ _２ は該第２の単位ベクトル、Ｔは該並進ベクトルであり、ここで、φ＝姿勢誤差であり、δＴ＝並進誤差であり、該カルマンフィルタが姿勢誤差φの解を得、δＴはＧＰＳによって計測される、システム。
10. 前記エピポーラ方程式は（ｘ_１ ^Ｌ×ｘ_２ ^Ｌ）・Ｃ_Ｎ ^ＬＴ^Ｎ＝０であり、ここで、ｘ_１ ^Ｌは前記第１の単位ベクトル、ｘ_２ ^Ｌは前記第２の単位ベクトル、Ｔ^Ｎは前記並進ベクトル、Ｃ_Ｎ ^Ｌは下記のようなナビゲーション（Ｎ）フレームから局所（Ｌ）フレームへの変換マトリクスであり、
    ここで、ｃ＝ｃｏｓ（ａ）であり、ｓ＝ｓｉｎ（ａ）であり、ａは解かれるべきヘディング角度である、請求項９に記載のシステム。
11. 前記結合プロセッサは、前記システムに対する前記ヘディングを直接的に計算するために、前記複数の照合された識別特徴のそれぞれの前記決定された補正されたヘディングを平均化する、請求項９に記載のシステム。
12. 前記ＧＰＳは、ＧＰＳデバイスとＧＰＳプロセッサとを備え、
    該ＧＰＳデバイスは、キャリア位相情報を提供し、
    該ＧＰＳプロセッサは、前記並進ベクトルを決定し、
    前記画像システムは、画像センサと画像プロセッサとを備え、
    該画像センサは、前記走行するビークルの走行期間中の前記第１の時点および前記第２の時点で、前記関心地域における前記複数の識別特徴をキャプチャし、
    該画像プロセッサは、該第１の時点でキャプチャされた該複数の識別特徴と該第２の時点でキャプチャされた該複数の識別特徴とを照合し、該複数の照合された識別特徴のそれぞれについて、該第１の時点に基づく、所与の照合された識別特徴に関連する前記第１の単位ベクトルと、該第２の時点に関連し、該所与の照合された識別特徴に関連する前記第２の単位ベクトルとを決定し、
    前記慣性計測システムは、慣性計測ユニット（ＩＭＵ）と慣性プロセッサとを備え、
    該ＩＭＵは、速度変化および姿勢変化を該慣性プロセッサに提供し、
    該慣性プロセッサは、位置、ヘディング、姿勢のそれぞれの評価値を決定する、請求項１に記載のシステム。
13. 画像システムと慣性システムとグローバルポジショニングシステム（ＧＰＳ）とを利用する走行するビークルのヘディングを決定する方法であって、該ヘディングは、該ビークルが走行する方向を表し、該方法は、
    一の関心地域において影像をキャプチャし、第１の時点で複数の識別特徴の座標を抽出することと、
    該一の関心地域において影像をキャプチャし、第２の時点で複数の識別特徴の座標を抽出することと、
    複数の照合された識別特徴を決定するために、該第１の時点でキャプチャされた該複数の識別特徴の座標と該第２の時点でキャプチャされた該複数の識別特徴の座標とを照合することと、
    該複数の照合された識別特徴のそれぞれについて、該第１の時点に基づく、所与の照合された識別特徴に関連する第１の単位ベクトルと、該第２の時点に関連し、該所与の照合された識別特徴に関連する第２の単位ベクトルとを提供することと、
    該第１の時点から該第２の時点までにキャプチャされたキャリア位相情報に基づいて、並進ベクトルを計算することと、
    該第１の単位ベクトルと該第２の単位ベクトルと該並進ベクトルとに基づいて、該複数の照合された識別特徴のそれぞれについてエピポーラ方程式を最小化することにより、補正されたヘディングを決定することと
    を包含しており、
    該エピポーラ方程式はδｒ＝Ｔ×（ｘ _１ ×ｘ _２ ）・φ＋（ｘ _１ ×ｘ _２ ）・δＴであり、ここで、ｘ _１ は該第１の単位ベクトル、ｘ _２ は該第２の単位ベクトル、Ｔは該並進ベクトルであり、ここで、φ＝姿勢誤差であり、δＴ＝並進誤差である、方法。
14. 前記エピポーラ方程式を最小化するために開始点で前記ＩＭＵによって提供される初期ヘディングを利用することをさらに包含している、請求項１３に記載の方法。
15. 前記エピポーラ方程式は（ｘ_１ ^Ｌ×ｘ_２ ^Ｌ）・Ｃ_Ｎ ^ＬＴ^Ｎ＝０であり、ここで、ｘ_１ ^Ｌは前記第１の単位ベクトル、ｘ_２ ^Ｌは前記第２の単位ベクトル、Ｔ^Ｎは前記並進ベクトル、Ｃ_Ｎ ^Ｌは下記のようなナビゲーション（Ｎ）フレームから局所（Ｌ）フレームへの変換マトリクスであり、
    ここで、ｃ＝ｃｏｓ（ａ）であり、ｓ＝ｓｉｎ（ａ）であり、ａは解かれるべきヘディング角度である、請求項１３に記載の方法。
16. 前記整合されたヘディングを直接的に計算するために、前記複数の照合された識別特徴のそれぞれの前記決定された補正されたヘディングを平均化することをさらに包含している、請求項１３に記載の方法。
17. カルマンフィルタが前記姿勢誤差φの解を得、前記δＴは前記ＧＰＳによって計測される、請求項１３に記載の方法。