特許 6987797 リアルタイムオンラインエゴモーション推定を有するレーザスキャナ

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6987797

(24)【登録日】2021年12月3日

(45)【発行日】2022年1月5日

(54)【発明の名称】リアルタイムオンラインエゴモーション推定を有するレーザスキャナ

(51)【国際特許分類】

   G01C 15/00 20060101AFI20211220BHJP

【ＦＩ】

   G01C15/00 101

【請求項の数】16

【全頁数】48

(21)【出願番号】特願2018-567003(P2018-567003)

(86)(22)【出願日】2017年3月7日

(65)【公表番号】特表2019-518222(P2019-518222A)

(43)【公表日】2019年6月27日

(86)【国際出願番号】US2017021120

(87)【国際公開番号】WO2017155970

(87)【国際公開日】20170914

【審査請求日】2020年2月10日

(31)【優先権主張番号】62/307,061

(32)【優先日】2016年3月11日

(33)【優先権主張国】US

(73)【特許権者】

【識別番号】518324267

【氏名又は名称】カールタ インコーポレイテッド

(74)【代理人】

【識別番号】100094569

【弁理士】

【氏名又は名称】田中 伸一郎

(74)【代理人】

【識別番号】100088694

【弁理士】

【氏名又は名称】弟子丸 健

(74)【代理人】

【識別番号】100103610

【弁理士】

【氏名又は名称】▲吉▼田 和彦

(74)【代理人】

【識別番号】100067013

【弁理士】

【氏名又は名称】大塚 文昭

(74)【代理人】

【識別番号】100086771

【弁理士】

【氏名又は名称】西島 孝喜

(74)【代理人】

【識別番号】100109070

【弁理士】

【氏名又は名称】須田 洋之

(74)【代理人】

【識別番号】100109335

【弁理士】

【氏名又は名称】上杉 浩

(74)【代理人】

【識別番号】100120525

【弁理士】

【氏名又は名称】近藤 直樹

(74)【代理人】

【識別番号】100139712

【弁理士】

【氏名又は名称】那須 威夫

(74)【代理人】

【識別番号】100151987

【弁理士】

【氏名又は名称】谷口 信行

(72)【発明者】

【氏名】ジャン ジー

(72)【発明者】

【氏名】シング サンジヴ

【審査官】 櫻井 仁

(56)【参考文献】

【文献】 特開２０１０−１７５４２３（ＪＰ，Ａ）

【文献】 特開２０１３−０９３７３８（ＪＰ，Ａ）

【文献】 特開２００４−３４８５７５（ＪＰ，Ａ）

【文献】 米国特許出願公開第２００８／００３３６４５（ＵＳ，Ａ１）

【文献】 特表２０１１−５２９５６９（ＪＰ，Ａ）

【文献】 特開２００７−２９８３３２（ＪＰ，Ａ）

【文献】 特開平０９−１４２２３６（ＪＰ，Ａ）

【文献】 特開２０１５−２００６１５（ＪＰ，Ａ）

【文献】 登録実用新案第３１７３４１９（ＪＰ，Ｕ）

【文献】 特表２０１２−５０７０１１（ＪＰ，Ａ）

【文献】 特開２００６−２７６０１２（ＪＰ，Ａ）

【文献】 特開２０１２−０６３１７３（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｃ １／００− １／１４

Ｇ０１Ｃ ５／００−１５／１４

Ｇ０１Ｓ ７／４８− ７／５１

Ｇ０１Ｓ １７／００−１７／９５

Ｇ０６Ｔ ７／００− ７／９０

(57)【特許請求の範囲】

【請求項1】

マッピングシステムであって、
慣性測定ユニットと、
カメラユニットと、
レーザ走査ユニットと、
前記慣性測定ユニットと、前記カメラユニットと、前記レーザ走査ユニットとに通信し、少なくとも１つのプロセッサと、少なくとも１つの１次メモリユニットと、少なくとも１つの２次メモリユニットとを含むコンピュータシステムと、
を含み、
前記少なくとも１つの１次メモリユニットは、前記少なくとも１つのプロセッサによって実行されるソフトウエアを格納し、該ソフトウエアは、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに第１の周波数での前記慣性測定ユニットからの慣性測定データに少なくとも部分的に基づいて少なくとも１つの第１の測定予測を計算させる第１の計算モジュールと、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第１の測定予測と第２の周波数での前記カメラユニットからの視覚測定データとに少なくとも部分的に基づいて少なくとも１つの第２の測定予測を計算させる第２の計算モジュールと、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第２の測定予測と第３の周波数での前記レーザ走査ユニットからのレーザ測距データとに基づいて少なくとも１つの第３の測定予測を計算させる第３の計算モジュールと、
を含み、
前記第１の計算モジュールは、前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第２の測定予測と前記少なくとも１つの第３の測定予測とに少なくとも部分的に基づいて前記少なくとも１つの第１の測定予測内のバイアス誤差を補正させるソフトウエアを更に含む、
ことを特徴とするマッピングシステム。

【請求項2】

前記第１の周波数は、前記第２の周波数よりも高く、該第２の周波数は、前記第３の周波数よりも高いことを特徴とする請求項１に記載のマッピングシステム。

【請求項3】

マッピングシステムであって、
慣性測定ユニットと、
カメラユニットと、
レーザ走査ユニットと、
前記慣性測定ユニットと、前記カメラユニットと、前記レーザ走査ユニットとに通信し、少なくとも１つのプロセッサと、少なくとも１つの１次メモリユニットと、少なくとも１つの２次メモリユニットとを含むコンピュータシステムと、
を含み、
前記少なくとも１つの１次メモリユニットは、前記少なくとも１つのプロセッサによって実行されるソフトウエアを格納し、該ソフトウエアは、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに第１の周波数での前記慣性測定ユニットからの慣性測定データに少なくとも部分的に基づいて少なくとも１つの第１の測定予測を計算させる第１の計算モジュールと、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第１の測定予測と第２の周波数での前記カメラユニットからの視覚測定データとに少なくとも部分的に基づいて少なくとも１つの第２の測定予測を計算させる第２の計算モジュールと、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第２の測定予測と第３の周波数での前記レーザ走査ユニットからのレーザ測距データとに基づいて少なくとも１つの第３の測定予測を計算させる第３の計算モジュールと、
を含み、
前記第２の計算モジュールは、前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに
前記視覚測定データが第１の測定期間中に劣化したか否かを決定させ、かつ
前記視覚測定データが前記第１の測定期間中に劣化したという決定時に、該第１の測定期間中の前記少なくとも１つの第１の測定予測に等しい該第１の測定期間中の前記少なくとも１つの第２の測定予測を計算させる、
更に別のソフトウエアを更に含む、
ことを特徴とするマッピングシステム。

【請求項4】

前記第３の計算モジュールは、前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに
前記レーザ測距データが第２の測定期間中に劣化したか否かを決定させ、かつ
前記レーザ測距データが前記第２の測定期間中に劣化したという決定時に、該第２の測定期間中の前記少なくとも１つの第２の測定予測に等しい該第２の測定期間中の前記少なくとも１つの第３の測定予測を計算させる、
別のソフトウエアを更に含むことを特徴とする請求項４に記載のマッピングシステム。

【請求項5】

マッピングシステムであって、
慣性測定ユニットと、
カメラユニットと、
レーザ走査ユニットと、
前記慣性測定ユニットと、前記カメラユニットと、前記レーザ走査ユニットとに通信し、少なくとも１つのプロセッサと、少なくとも１つの１次メモリユニットと、少なくとも１つの２次メモリユニットとを含むコンピュータシステムと、
を含み、
前記少なくとも１つの１次メモリユニットは、前記少なくとも１つのプロセッサによって実行されるソフトウエアを格納し、該ソフトウエアは、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに第１の周波数での前記慣性測定ユニットからの慣性測定データに少なくとも部分的に基づいて少なくとも１つの第１の測定予測を計算させる第１の計算モジュールと、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第１の測定予測と第２の周波数での前記カメラユニットからの視覚測定データとに少なくとも部分的に基づいて少なくとも１つの第２の測定予測を計算させる第２の計算モジュールと、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第２の測定予測と第３の周波数での前記レーザ走査ユニットからのレーザ測距データとに基づいて少なくとも１つの第３の測定予測を計算させる第３の計算モジュールと、
を含み、
前記少なくとも１つの１次メモリユニットは、第１のセットのボクセル及び第２のセットのボクセルを格納し、
前記第１のセットのボクセル及び前記第２のセットのボクセルは、前記少なくとも１つの第３の測定予測に基づいており、
前記第１のセットのボクセルの各ボクセルが、空間の第１の容積に対応し、
前記第２のセットのボクセルの各ボクセルが、空間の第２の容積に対応し、
空間の前記第２の容積は、空間の前記第１の容積よりも小さく、
前記第１のセットのボクセルの各ボクセルが、前記第２のセットのボクセルの複数のボクセルにマップ可能である、
ことを特徴とするマッピングシステム。

【請求項6】

前記少なくとも１つの２次メモリユニットは、前記少なくとも１つの第３の測定予測から発生したポイントクラウドデータを格納することを特徴とする請求項１に記載のマッピングシステム。

【請求項7】

モバイルユニットを更に含み、
前記慣性測定ユニットは、前記モバイルユニットの一部を形成し、
前記カメラユニットは、前記モバイルユニットの一部を形成し、
前記レーザ走査ユニットは、前記モバイルユニットの一部を形成し、
前記コンピュータシステムは、前記モバイルユニットの一部を形成する、
ことを特徴とする請求項１に記載のマッピングシステム。

【請求項8】

前記モバイルユニットは、該モバイルユニットを案内するためのナビゲーションシステムを含み、
前記ナビゲーションシステムは、前記モバイルユニットを案内するために前記少なくとも１つの第３の測定予測を使用する、
ことを特徴とする請求項７に記載のマッピングシステム。

【請求項9】

マッピングシステムであって、
慣性測定ユニットと、
カメラユニットと、
レーザ走査ユニットと、
前記慣性測定ユニットと、前記カメラユニットと、前記レーザ走査ユニットとに通信し、少なくとも１つのプロセッサと、少なくとも１つの１次メモリユニットと、少なくとも１つの２次メモリユニットとを含むコンピュータシステムと、
を含み、
前記少なくとも１つの１次メモリユニットは、前記少なくとも１つのプロセッサによって実行されるソフトウエアを格納し、該ソフトウエアは、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに第１の周波数での前記慣性測定ユニットからの慣性測定データに少なくとも部分的に基づいて少なくとも１つの第１の測定予測を計算させる第１の計算モジュールと、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第１の測定予測と第２の周波数での前記カメラユニットからの視覚測定データとに少なくとも部分的に基づいて少なくとも１つの第２の測定予測を計算させる第２の計算モジュールと、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第２の測定予測と第３の周波数での前記レーザ走査ユニットからのレーザ測距データとに基づいて少なくとも１つの第３の測定予測を計算させる第３の計算モジュールと、
を含み、
前記第３の計算モジュールは、走査適合アルゴリズムを使用して前記少なくとも１つの第３の測定予測を計算し、
前記少なくとも１つのプロセッサは、複数の処理スレッドを含み、
前記少なくとも１つの１次メモリユニットは、前記少なくとも１つのプロセッサによって実行された時に、
第１のスレッドが、前記レーザ測距データの第１の走査を走査適合させるように割り当てられ、
前記第１のスレッドが、前記第１の走査後の時点から前記レーザ測距データの第２の走査を走査適合させるように割り当てられ、それによって該第１のスレッドが、該第２の走査の到着の前に該第１の走査を処理し、かつ
第２のスレッドが、前記第１のスレッドが前記第２の走査の前記到着の前に前記第１の走査を処理できない時に、前記レーザ測距データの前記第２の走査を走査適合させるように割り当てられる、
ように、前記複数の処理スレッドによる前記レーザ測距データの走査の処理を管理するソフトウエアを格納する、
ことを特徴とするマッピングシステム。

【請求項10】

マッピングシステムであって、
慣性測定ユニットと、
カメラユニットと、
レーザ走査ユニットと、
前記慣性測定ユニットと、前記カメラユニットと、前記レーザ走査ユニットとに通信し、少なくとも１つのプロセッサと、少なくとも１つの１次メモリユニットと、少なくとも１つの２次メモリユニットとを含むコンピュータシステムと、
を含み、
前記少なくとも１つの１次メモリユニットは、前記少なくとも１つのプロセッサによって実行されるソフトウエアを格納し、該ソフトウエアは、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに第１の周波数での前記慣性測定ユニットからの慣性測定データに少なくとも部分的に基づいて少なくとも１つの第１の測定予測を計算させる第１の計算モジュールと、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第１の測定予測と第２の周波数での前記カメラユニットからの視覚測定データとに少なくとも部分的に基づいて少なくとも１つの第２の測定予測を計算させる第２の計算モジュールと、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第２の測定予測と第３の周波数での前記レーザ走査ユニットからのレーザ測距データとに基づいて少なくとも１つの第３の測定予測を計算させる第３の計算モジュールと、
を含み、
前記少なくとも１つの第１の測定予測は、複数の自由度を含む姿勢の少なくとも１つの自由度の予測を含み、
前記少なくとも１つの第２の測定予測は、複数の自由度を含む前記姿勢の少なくとも１つの自由度の予測を含み、かつ
前記少なくとも１つの第３の測定予測は、複数の自由度を含む前記姿勢の少なくとも１つの自由度の予測を含む、
ことを特徴とするマッピングシステム。

【請求項11】

前記第２の計算モジュールは、前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに
前記視覚測定データのいずれかが第１の測定期間中に劣化したか否かを決定させ、かつ
前記視覚測定データのいずれかが前記第１の測定期間中に劣化したという決定時に、非劣化視覚測定データに対する前記少なくとも１つの第２の測定予測の複数の自由度を計算させる、
別のソフトウエアを更に含むことを特徴とする請求項１０に記載のマッピングシステム。

【請求項12】

前記第３の計算モジュールは、前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに
前記レーザ測距データのいずれかが第２の測定期間中に劣化したか否かを決定させ、かつ
前記レーザ測距データのいずれかが前記第２の測定期間中に劣化したという決定時に、非劣化レーザ測距データに対する前記少なくとも１つの第３の測定予測の複数の自由度を計算させる、
別のソフトウエアを更に含むことを特徴とする請求項１０に記載のマッピングシステム。

【請求項13】

前記第１の計算モジュールは、前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第１の測定予測の前記少なくとも１つの自由度と、前記少なくとも１つの第２の測定予測の前記少なくとも１つの自由度と、前記少なくとも１つの第３の測定予測の前記少なくとも１つの自由度とに少なくとも部分的に基づいて前記姿勢のバイアス誤差を補正させる別のソフトウエアを更に含むことを特徴とする請求項１０に記載のマッピングシステム。

【請求項14】

前記第２の計算モジュールは、前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに
前記視覚測定データのいずれかが第１の測定期間中に劣化したか否かを決定させ、かつ
前記視覚測定データのいずれかが前記第１の測定期間中に劣化したという決定時に、非劣化視覚測定データに対する前記少なくとも１つの第２の測定予測の複数の自由度を計算させる、
別のソフトウエアを更に含み、かつ
前記第３の計算モジュールは、前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに
前記レーザ測距データのいずれかが第２の測定期間中に劣化したか否かを決定させ、かつ
前記レーザ測距データのいずれかが前記第２の測定期間中に劣化したという決定時に、非劣化レーザ測距データに対する前記少なくとも１つの第３の測定予測の複数の自由度を計算させる、
別のソフトウエアを更に含む、
ことを特徴とする請求項１０に記載のマッピングシステム。

【請求項15】

前記第１の計算モジュールは、前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第１の測定予測の前記少なくとも１つの自由度と、前記少なくとも１つの第２の測定予測の前記少なくとも１つの自由度と、該少なくとも１つの第２の測定予測の自由度ではない前記少なくとも１つの第３の測定予測の少なくとも１つの自由度とに基づいて前記姿勢のバイアス誤差を補正させる別のソフトウエアを更に含むことを特徴とする請求項１４に記載のマッピングシステム。

【請求項16】

マッピングシステムであって、
慣性測定ユニットと、
カメラユニットと、
レーザ走査ユニットと、
前記慣性測定ユニットと、前記カメラユニットと、前記レーザ走査ユニットとに通信し、少なくとも１つのプロセッサと、少なくとも１つの１次メモリユニットと、少なくとも１つの２次メモリユニットとを含むコンピュータシステムと、
を含み、
前記少なくとも１つの１次メモリユニットは、前記少なくとも１つのプロセッサによって実行されるソフトウエアを格納し、該ソフトウエアは、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに第１の周波数での前記慣性測定ユニットからの慣性測定データに少なくとも部分的に基づいて少なくとも１つの第１の測定予測を計算させる第１の計算モジュールと、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第１の測定予測と第２の周波数での前記カメラユニットからの視覚測定データとに少なくとも部分的に基づいて少なくとも１つの第２の測定予測を計算させる第２の計算モジュールと、
前記少なくとも１つのプロセッサによって実行された時に該少なくとも１つのプロセッサに前記少なくとも１つの第２の測定予測と第３の周波数での前記レーザ走査ユニットからのレーザ測距データとに基づいて少なくとも１つの第３の測定予測を計算させる第３の計算モジュールと、
を含み、
前記第１の周波数は、前記第２の周波数よりも高く、該第２の周波数は、前記第３の周波数よりも高く、
前記第２の周波数は、前記カメラユニットからのフレーム速度である、
ことを特徴とするマッピングシステム。

【発明の詳細な説明】

【技術分野】

【0001】

〔優先権の表記〕
この出願は、引用により本明細書にその開示が全体的に全ての目的に対して組み込まれている２０１６年３月１１日出願の「リアルタイムオンラインエゴモーション推定を有するレーザスキャナ」という名称の米国仮特許出願第６２／３０７，０６１号の利益を主張するものである。

【背景技術】

【0002】

自律移動デバイスは、それがそこで作動する地形に関する情報を要求する場合がある。そのようなデバイスは、地形を提示する予め定められたマップ及びそこに見出される場合があるあらゆる障害物に頼る場合がある。これに代えて、デバイスは、リアルタイムデータを提供するために１又は２以上のセンサを備えたコンピュータベースのマッピングシステムを含む固定又は運動中のいずれかの間にその地形をマップする機能を有する場合がある。モバイルコンピュータベースマッピングシステムは、その位置の変化を経時的に推定する（走行距離計）及び／又は３次元空間のポイントクラウドのような３次元マップ表現を発生させることができる。

【0003】

例示的マッピングシステムは、マップをそこから構築することができるデータを提供する様々なセンサを含むことができる。一部のマッピングシステムは、１つのそのようなセンサとしてステレオカメラシステムを使用する場合がある。これらのシステムは、モーション推定のスケールを決定するために基準として２つのカメラの間のベースラインから恩恵を得ている。単眼システムは、追加のセンサからデータを受信する又はデバイスのモーションに関して仮定を行うことなしでは画像のスケールを分解することができない場合があるので、２鏡システムが単眼システムよりも好ましい。近年では、ＲＧＢ−Ｄカメラが研究分野で人気を得ている。そのようなカメラは、個々のピクセルに関連付けられた深度情報を提供することができ、従って、スケールの決定を助けることができる。しかし、ＲＧＢ−Ｄカメラを含む一部の方法は、深度情報のカバレージを有する画像区域のみを使用する場合があり、これは、特に深度が疎にのみ利用可能である可能性がある開放環境では大きい画像区域が浪費されることをもたらす場合がある。

【0004】

マッピングシステムの他の例では、スケール制約をＩＭＵ加速から提供することができるようにＩＭＵを１又は２以上のカメラに結合することができる。一部の例では、単眼鏡カメラをカルマンフィルタによってＩＭＵにきつく又は緩く結合することができる。他のマッピングシステムは、モバイルシステムのモーションに関して解くために最適化方法を使用する場合がある。

【0005】

マッピングシステムの代替例は、モーション推定のためのレーザスキャナの使用を含むことができる。しかし、レーザの走査速度から、そのようなデータの使用の難しさが生じる場合がある。システムが移動している間は、固定位置レーザスキャナとは異なってレーザポイントは、スキャナの相対移動によって影響を受ける。従って、この移動の影響は、異なる時間にシステムに到着するレーザポイントの要因である場合がある。その結果、走査速度がマッピングシステムのモーションに対して遅い時は、レーザの外部モーションに起因して走査歪みが存在する場合がある。モーション効果は、レーザ自体によって補償することができるが、補償は、必要な補正を提供するための独立したモーションモデルを要求する場合がある。一例として、モーションは、一定速度として又はガウス過程としてモデル化することができる。一部の例では、ＩＭＵは、モーションモデルを提供することができる。そのような方法は、レーザポイントクラウドによって形成された空間−時間パッチを適合させてセンサモーションを推定し、かつオフラインバッチ最適化でＩＭＵバイアスを補正する。

【0006】

ローリングシャッターカメラの使用では、モーション歪みの類似の問題が見出される場合がある。具体的には、画像ピクセルが経時的に継続して受信される場合があり、カメラの外因性モーションによって引き起こされる画像歪みをもたらす。一部の例では、視覚的走行距離計測方法は、ピクセルの読み出し時間が与えられるとローリングシャッター効果を補償するためにＩＭＵを使用することができる。

【0007】

一部の例では、ＧＰＳ／ＩＮＳ技術を使用してモバイルマッピングデバイスの位置を決定することができる。しかし、高精度ＧＰＳ／ＩＮＳソリューションは、用途がＧＰＳ拒否型、軽量、又は費用重視である時に実用的ではない場合がある。正確なＧＰＳマッピングは、ＧＰＳ受信機と少なくとも４つのＧＰＳ衛星（５が好ましいが）の間の見通し通信を要求することが認識されている。一部の環境では、例えば陸橋及び他の障害物を含む場合がある都会環境では、４つの衛星から歪みのない信号を受信することは困難であると考えられる。

【発明の概要】

【発明が解決しようとする課題】

【0008】

すなわち、特にマッピングデバイスが運動中である間は、自律マッピングデバイスの周りの地形のロバストマップを発生させるために光学デバイスからのデータを他のモーション測定デバイスと融合させることに関連付けられたいくつかの技術的な課題があることを認めることができる。以下に開示するのは、光学マッピング情報を取得し、かつ歪みを低減したロバストマップを生成することができるマッピングデバイスの方法及びシステムである。

【課題を解決するための手段】

【0009】

この節での例は、単に一部の可能な実施形態を表すものであるが、この出願で開示する全ての可能な実施形態、要素の組合せ、又は発明を反映するものではない。一例では、マッピングシステムは、慣性測定ユニットと、カメラユニットと、レーザ走査ユニットと、慣性測定ユニット、カメラユニット、及びレーザ走査ユニットと通信するコンピュータシステムとを含むことができる。コンピュータシステムは、少なくとも１つのプロセッサ、少なくとも１つの１次メモリユニット、及び少なくとも１つの２次メモリユニットから構成することができる。１次メモリユニットは、少なくとも１つのプロセッサによって実行されるソフトウエアを格納することができ、ソフトウエアは、少なくとも１つのプロセッサによって実行された時に少なくとも１つのプロセッサに第１の周波数での慣性測定ユニットからの慣性測定データに基づいて第１の測定予測を計算させる第１の計算モジュールと、少なくとも１つのプロセッサによって実行された時に少なくとも１つのプロセッサに第１の測定予測及び第２の周波数でのカメラユニットからの視覚測定データに基づいて第２の測定予測を計算させる第２の計算モジュールと、少なくとも１つのプロセッサによって実行された時に少なくとも１つのプロセッサに第２の測定予測及び第３の周波数でのレーザ走査ユニットからのレーザ測距データに基づいて第３の測定予測を計算させる第３の計算モジュールとを含むことができる。

【0010】

マッピングシステムの例では、第１の計算モジュールは、少なくとも１つのプロセッサによって実行された時に少なくとも１つのプロセッサに第２の測定予測及び第３の測定予測に基づいて第１の測定予測内のバイアス誤差を補正させるソフトウエアを更に含むことができる。

【0011】

マッピングシステムの例では、第１の周波数は、第２の周波数よりも高く、第２の周波数は、第３の周波数よりも高い。

【0012】

マッピングシステムの例では、第２の計算モジュールは、少なくとも１つのプロセッサによって実行された時に少なくとも１つのプロセッサに第１の測定期間中に視覚測定データが劣化したか否かを決定させ、かつ視覚測定データが第１の測定期間中に劣化したという決定時に第１の測定期間中の第１の測定予測に等しい第１の測定期間中の第２の測定予測を計算させるソフトウエアを更に含むことができる。

【0013】

マッピングシステムの例では、第３の計算モジュールは、少なくとも１つのプロセッサによって実行された時に少なくとも１つのプロセッサにレーザ測距データが第２の測定期間中に劣化したか否かを決定させ、かつレーザ測距データが第２の測定期間中に劣化したという決定時に第２の測定期間中の第２の測定予測に等しい第２の測定期間中の第３の測定予測を計算させるソフトウエアを更に含むことができる。

【0014】

マッピングシステムの例では、１次メモリデバイスは、第１及び第２のセットのボクセルを格納することができ、第１及び第２のセットのボクセルは、第３の予測測定に基づいている。第１のセットのボクセルの各ボクセルは、空間の第１の容積に対応することができ、第２のセットのボクセルの各ボクセルは、空間の第２の容積に対応することができる。空間の第２の容積は、空間の第１の容積よりも小さい場合があり、第１のセットのボクセルの各ボクセルは、第２のセットのボクセルの複数のボクセルにマップ可能である場合がある。

【0015】

マッピングシステムの例では、２次メモリユニットは、第３の予測測定から発生したポイントクラウドデータを格納することができる。

【0016】

一例では、マッピングシステムは、モバイルユニットを更に含むことができ、慣性測定ユニットが、モバイルユニット上にあり、カメラユニットが、モバイルユニット上にあり、レーザ走査ユニットが、モバイルユニット上にあり、コンピュータシステムが、モバイルユニット上にある。

【0017】

マッピングシステムの例では、モバイルユニットは、モバイルユニットを案内するためのナビゲーションシステムを含むことができ、ナビゲーションシステムは、第３の測定予測を使用して自律モバイルユニットを案内することができる。

【0018】

マッピングシステムの例では、第３の計算モジュールは、走査適合アルゴリズムを使用して第３の測定予測を計算することができる。少なくとも１つのプロセッサは、複数の処理スレッドを含むことができる。１次メモリデバイスは、少なくとも１つのプロセッサによって実行された時に第１のスレッドがレーザ測距データの第１の走査を走査適合させるように割り当てられるような複数のスレッドによってレーザ測距データの走査の処理を管理することができるソフトウエアを格納することができる。第１のスレッドは、第１のスレッドが第２の走査の到着前に第１の走査を処理することができる時に、第１の走査後の時点からレーザ測距データの第２の走査を走査適合させるように割り当てることができる。第２のスレッドは、第１のスレッドが第２の走査の到着前に第１の走査を処理できない時に、レーザ測距データの第２の走査を走査適合させるように割り当てることができる。

【0019】

マッピングシステムの例では、慣性測定ユニット、カメラユニット、及びレーザ走査ユニットは、走査データのダウンサンプルバージョンが３次元表現で提示される対話型ディスプレイにコンピュータシステムを通じてインタフェース接続することができる。

【0020】

例示的かつ非限定的実施形態により、マッピングシステムは、慣性測定ユニットと、カメラユニットと、レーザ走査ユニットと、慣性測定ユニット、カメラユニット、及びレーザ走査ユニットと通信するコンピュータシステムとを含み、コンピュータシステムは、少なくとも１つのプロセッサ、少なくとも１つの１次メモリユニット、及び少なくとも１つの２次メモリユニットを含み、１次メモリユニットは、少なくとも１つのプロセッサによって実行されるソフトウエアを格納し、ソフトウエアは、少なくとも１つのプロセッサによって実行された時に少なくとも１つのプロセッサに第１の周波数での慣性測定ユニットからの慣性測定データに少なくとも部分的に基づく少なくとも１つの第１の測定予測を計算させる第１の計算モジュールと、少なくとも１つのプロセッサによって実行された時に少なくとも１つのプロセッサに少なくとも１つの第１の測定予測及び第２の周波数でのカメラユニットからの視覚測定データに少なくとも部分的に基づく少なくとも１つの第２の測定予測を計算させる第２の計算モジュールと、少なくとも１つのプロセッサによって実行された時に少なくとも１つのプロセッサに少なくとも１つの第２の測定予測及び第３の周波数でのレーザ走査ユニットからのレーザ測距データに基づく少なくとも１つの第３の測定予測を計算させる第３の計算モジュールとを含む。

【0021】

例示的かつ非限定的実施形態により、マッピングシステムは、双方向情報フローの問題に対処するためにモジュール式システム構造から構成される。具体的には、３つのモジュールが、粗から細のデータの問題に段階的に対処する。データ処理フローは、ＩＭＵ予測モジュールから視覚−慣性走行距離計測モジュールに、更に走査適合精緻化モジュールに進むことができ、一方でフィードバックフローが、ＩＭＵのバイアスを補正するために逆の順に発生する。

【0022】

例示的かつ非限定的実施形態により、マッピングシステムは、動的に再構成可能である。例えば、視覚特徴が視覚−慣性走行距離計測に不十分である場合に、ＩＭＵ予測モジュールは、視覚−慣性走行距離計測モジュールを（部分的に）バイパスし、ローカルにレーザポイントを記録する。他方、環境構造が走査適合に不十分である場合に、視覚−慣性走行距離計測出力は、走査適合精緻化モジュールを（部分的に）バイパスし、マップ上にレーザポイントを記録する。

【0023】

例示的かつ非限定的実施形態により、マッピングシステムは、ＩＭＵバイアス補正のための優先度フィードバックを使用する。例えば、視覚−慣性走行距離計測モジュール及び走査適合精緻化モジュールの両方は、ＩＭＵバイアスを補正するためにＩＭＵ予測モジュールにフィードバックを提供することができる。フィードバックは、組み合わされて視覚−慣性走行距離計測モジュールに優先度を与えることができる。換言すると、走査適合精緻化モジュールからのフィードバックは、視覚−慣性走行距離計測モジュールが劣化した方向に視覚−慣性走行距離計測モジュールを補償する。

【0024】

例示的かつ非限定的実施形態により、マッピングシステムは、マップの２層ボクセル表現を使用する。第１の層は、大きいボクセルから構成される。この層は、マップストレージのためのものである。センサに近い各大きいボクセルに対して、ボクセルは、走査適合のためにマップを正確に取り出すための小さいボクセルの第２の層を含有する。

【0025】

例示的かつ非限定的実施形態により、マッピングシステムは、走査適合のマルチスレッド処理を使用する。走査適合は、ＫＤツリー構築、ポイントクエリ、及び非線形最適化のための逆行列などを利用することができる。ｏｐｅｎＭＰのような標準的な並行処理は、ポイントクエリを加速することができるだけであり、全体時間を実質的に低減するのに寄与しない。対照的に、本発明のシステムは、各スレッド上で異なる走査を処理する。換言すると、４つのスレッドは、１つの走査の代わりに４つの連続走査を処理する。

【図面の簡単な説明】

【0026】

【図1】マッピングシステムの実施形態を示すブロック図である。

【図2】図１のマッピングシステムの３つの計算モジュール及びそれらのそれぞれのフィードバック特徴のブロック図の実施形態を示す図である。

【図3】位置情報をマップの中に精緻化するためのカルマンフィルタモデルの実施形態を示す図である。

【図4】位置情報をマップの中に精緻化するための因子グラフ最適化モデルの実施形態を示す図である。

【図5】視覚−慣性走行距離計測サブシステムの実施形態を示す図である。

【図6】走査適合サブシステムの実施形態を示す図である。

【図7A】粗詳細解像度を有する大面積マップの実施形態を示す図である。

【図7B】細詳細解像度を有する小面積マップの実施形態を示す図である。

【図8A】マルチスレッド走査適合の実施形態を示す図である。

【図8B】単一スレッド走査適合の実施形態を示す図である。

【図9A】視覚−慣性走行距離計測ユニットからのフィードバックデータがデータ劣化に起因して抑制される３つの計算モジュールのブロック図の実施形態を示す図である。

【図9B】走査適合ユニットからのフィードバックデータがデータ劣化に起因して抑制される３つの計算モジュールの実施形態を示す図である。

【図10】視覚−慣性走行距離計測ユニット及び走査適合ユニットからのフィードバックデータがデータ劣化に起因して部分的に抑制される３つの計算モジュールの実施形態を示す図である。

【図11】モバイルマッピングデバイスの推定される軌跡の実施形態を示す図である。

【図12】例示的かつ非限定的実施形態による双方向情報フローを示す図である。

【図13a】例示的かつ非限定的実施形態による動的再構成可能システムを示す図である。

【図13b】例示的かつ非限定的実施形態による動的再構成可能システムを示す図である。

【図14】例示的かつ非限定的実施形態によるＩＭＵバイアス補正のため優先度フィードバックを示す図である。

【図15a】例示的かつ非限定的実施形態によるマップの２層ボクセル表現を示す図である。

【図15b】例示的かつ非限定的実施形態によるマップの２層ボクセル表現を示す図である。

【図16a】例示的かつ非限定的実施形態による走査適合のマルチスレッド処理を示す図である。

【図16b】例示的かつ非限定的実施形態による走査適合のマルチスレッド処理を示す図である。

【発明を実施するための形態】

【0027】

１つの一般的態様では、本発明は、位置の変化を経時的に推定する（走行距離計）及び／又は３次元空間のポイントクラウドのような３次元マップ表現を発生させるモバイルコンピュータベースマッピングシステムに関する。本発明のマッピングシステムは、以下に限定されるものではないが、慣性測定ユニット（ＩＭＵ）、カメラ、及び／又は３Ｄレーザスキャナを含む複数のセンサを含むことができる。それはまた、経時的にシステムの位置の変化を推定する及び／又は周囲環境のマップ表現を発生させるためにセンサからの出力を処理するように構成された複数のセンサと通信する少なくとも１つのプロセッサを有するコンピュータシステムを含むことができる。マッピングシステムは、高密度の正確な３Ｄマッピング記録と共に、高周波数、低周波数、オンライン、リアルタイムのエゴモーション推定を可能にすることができる。本発明の開示の実施形態は、同時位置付け及びマッピング（ＳＬＡＭ）システムを含むことができる。ＳＬＡＭシステムは、ＧＰＳ独立であってリアルタイム同時位置付け及びマッピングを提供する多次元（例えば、３Ｄ）レーザ走査及び範囲測定システムを含むことができる。ＳＬＡＭシステムは、環境内の物体からのレーザ走査の反射から生じる高精度ポイントクラウドのデータを発生及び管理することができる。ポイントクラウド内のポイントのうちのいずれかの移動は、経時的に正確に追跡され、それによってＳＬＡＭシステムは、場所の基準ポイントとしてポイントクラウドのポイントを使用してそれが環境を通って移動した時の場所及び向きの正確な理解を維持することができる。

【0028】

一実施形態では、モバイルマッピングシステムの位置及びモーションの解像度は、一連の粗対細更新で逐次精緻化することができる。非限定例では、離散計算モジュールを使用して、急速更新速度を有する粗解像度からより遅い更新速度を有する細解像度までモバイルマッピングシステムの位置及びモーションを更新することができる。例えば、ＩＭＵデバイスは、データを第１の計算モジュールに提供して高速更新速度でマッピングシステムのモーション又は位置を予測することができる。視覚−慣性走行距離計測システムは、データを第２の計算モジュールに提供して低速更新速度でマッピングシステムのモーション又は位置解像度を改善することができる。これに加えて、レーザスキャナは、第３の計算走査適合モジュールにデータを提供して、更に低速の更新速度でモーション推定値を更に精緻化してマップを記録することができる。一非限定例では、細位置データ及び／又はモーション解像度データを処理するように構成された計算モジュールからのデータは、粗位置データ及び／又はモーション解像度データを処理するように構成された計算モジュールにフィードバックすることができる。別の非限定例では、計算モジュールは、欠陥、間違い、不完全、又は存在しないデータのソースになっているセンサに関連付けられた計算モジュールを自動的にバイパスすることにより、センサ劣化の問題に対処するための欠陥許容値を組み込むことができる。すなわち、マッピングシステムは、高度に動的なモーションの存在下で、並びに暗く、テクスチャがなく、かつ構造のない環境で作動することができる。

【0029】

多くがオフラインバッチシステムである既存のマップ発生技術とは対照的に、本明細書に開示するマッピングシステムは、運動している間にリアルタイムで作動してマップを発生することができる。この機能は、２つの実際的な利点を提供する。第１に、ユーザは、三脚又は他の非固定マウント上に固定されたスキャナに制限されない。代わりに、本明細書に開示するマッピングシステムは、モバイルデバイスに関連付けることができ、それによってリアルタイムでマップすることができる環境の範囲を増大する。第２に、リアルタイム特徴は、データが収集される間に現在マップされている区域のフィードバックをユーザに与えることができる。オンライン発生マップはまた、自律ナビゲーション及び障害物回避に関してロボット又は他のデバイスを補助することができる。一部の非限定的実施形態では、そのようなナビゲーション機能は、マッピングシステム自体に組み込むことができる。代替非限定的実施形態では、マップデータは、外部ソースのマップを要求する場合があるナビゲーション機能を有する追加のロボットに提供することができる。

【0030】

３Ｄモデリング、シーンマッピング、及び環境推論のようなセンサのいくつかの潜在的用途が存在する。本発明のマッピングシステムは、更に別の処理のための入力としてポイントクラウドを取る他のアルゴリズムのためのポイントクラウドマップを提供することができる。更に、本発明のマッピングシステムは、屋内及び屋外の両方で機能することができる。そのような実施形態は、外部照明を必要とせず、暗闇で作動することができる。カメラを有する実施形態は、高速モーションを扱うことができ、カメラからの画像でレーザポイントクラウドをカラー化することができるが、外部照明が必要である場合がある。ＳＬＡＭシステムは、ユーザが歩いている、自転車に乗っている、運転している、飛んでいる、及びその組合せの時のようなユーザが環境を運動中の時にポイントクラウドをリアルタイムで構築及び維持することができる。マップは、マップ器が環境を進んでいる時にリアルタイムで構成される。ＳＬＡＭシステムは、ポイントとして数千の特徴を追跡することができる。マップ器が移動すると、ポイントが追跡されてモーションの推定を可能にする。従って、ＳＬＡＭシステムは、リアルタイムでかつＧＰＳのような外部位置付け技術に関係なく作動する。実施形態では、物体のような環境の複数の（多くの場合に非常に多数の）特徴が三角測量のためのポイントとして使用され、システムは、リアルタイムで多くの場所及び向き計算を実行及び更新し、ＳＬＡＭシステムが環境を移動した時の位置及び向きの正確な現在の推定値を維持する。実施形態では、環境内の特徴の相対的なモーションを使用して移動特徴（人、車両、及び他の移動品目など）から固定特徴（壁、扉、窓、家具、及び作り付け備品など）を区別することができ、それによって固定特徴を位置及び向き計算に使用することができる。水中ＳＬＡＭシステムは、減衰を低減するために青緑色のレーザを使用する場合がある。

【0031】

マッピングシステム設計は、エゴモーション推定のドリフトがモジュール固有の周波数よりも低い周波数を有するという観察に従っている。３つの計算モジュールは、従って、減少する周波数の順番で並べられる。高周波モジュールは、積極的なモーションを扱うように特化されるが、低周波モジュールは、以前のモジュールのドリフトを打ち消す。逐次処理は計算にも有利であり、前にあるモジュールは、計算をあまり費やすことなく高周波で実行し、完全な処理のために後ろのモジュールに十分な時間を与える。マッピングシステムは、従って、リアルタイムでオンライン実行しながら高レベルの精度を達成することができる。

【0032】

更に、システムは、センサ劣化を扱うように構成することができる。カメラが機能しない場合（例えば、暗闇、大きい照明の変化、又はテクスチャの少ない環境により）、又はレーザが機能しない場合（例えば、構造の少ない環境により）、対応するモジュールをバイパスすることができ、システムの残りは、確実に機能するように交互配置することができる。システムは、多数の実験を通じて試験され、結果は、システムが環境的劣化及び積極的なモーションに対してナビゲーションの数キロメートルにわたる高精度及びロバスト性を生じることができることを示している。

【0033】

以下に開示するモジュール式マッピングシステムは、多層最適化構造を使用することにより、距離センサ、視覚センサ、及び慣性センサからのデータを処理してモーション推定及びマッピングを実行するように構成される。モジュール式マッピングシステムは、以下を含むことができる特徴を組み込むことにより、高精度、ロバスト性、及び低ドリフトを達成することができる：
・計算モジュールを動的に再構成する機能、
・計算モジュールの故障モードを完全に又は部分的にバイパスし、かつセンサ及び／又はセンサデータの劣化を扱う方式で残りのモジュールからのデータを組合せ、それによって環境的に誘導されたデータ劣化及びモバイルマッピングシステムの積極的なモーションに対処する機能、及び
・リアルタイムの性能を提供するために計算モジュールを協働的に統合する機能。

【0034】

本明細書に開示するのは、３Ｄレーザ、カメラ、及びＩＭＵからのデータによってオンラインエゴモーション推定をするマッピングシステムである。推定されたモーションは、横断された環境のマップを構築するためにレーザポイントを記録する。多くの現実世界の用途では、エゴモーション推定及びマッピングは、リアルタイムで行わなくてはならない。自律ナビゲーションシステムでは、マップは、モーション計画及び障害物回避に重要であり、同時にモーション推定は、車両制御及び操縦に重要である。

【0035】

図１は、本発明の一実施形態によるマッピングシステム１００の単純化ブロック図を示している。特定の構成要素を以下に開示するが、これらの構成要素は例としてだけ提示され、他の均等物又は類似の構成要素に関して制限するものではない。例示的なシステムは、Ｘｓｅｎｓ（記録商標）ＭＴｉ−３０ＩＭＵのようなＩＭＵシステム１０２、ＩＤＳ（記録商標）ＵＩ−１２２０ＳＥモノクロカメラのようなカメラシステム１０４、及びＶｅｌｏｄｙｎｅＰＵＣＫ（記録商標）ＶＬＰ−１６レーザスキャナのようなレーザスキャナ１０６を含む。ＩＭＵ１０２は、ｘ−ｙ−ｚ加速度計、ロール・ピッチ・ヨージャイロスコープ、及び磁気探知器の１又は２以上から得られた慣性モーションデータを提供することができ、第１の周波数での慣性データを提供することができる。一部の非限定例では、第１の周波数は、約２００Ｈｚとすることができる。カメラシステム１０４は、約７５２ｘ４８０ピクセルの解像度、７６°水平視野（ＦＯＶ）、及び第２の周波数でのフレーム取り込み速度を有することができる。一部の非限定例では、フレーム取り込み速度は、約５０Ｈｚの第２の周波数で作動することができる。レーザスキャナ１０６は、３６０°水平ＦＯＶ、３０°垂直ＦＯＶを有することができ、レーザスピン速度を表す第３の周波数で３０万ポイント／秒を受信することができる。一部の非限定例では、第３の周波数は、約５Ｈｚとすることができる。図１に示すように、レーザスキャナ１０６を符号器１０９を組み込むモータ１０８に接続してモータ回転角度を測定することができる。一非限定例では、レーザモータ符号器１０９は、約０．２５°の分解能で作動させることができる。

【0036】

ＩＭＵ１０２、カメラ１０４、レーザスキャナ１０６、及びレーザスキャナモータ符号器１０９は、１又は２以上のプロセッサ１３４を有するあらゆるコンピュータデバイスとすることができるコンピュータシステム１１０と、十分な処理パワーを有する関連のメモリ１２０、１６０及び望ましい走行距離計測方法及び／又はマッピングを実行するためのメモリとにデータ通信することができる。例えば、２．６ＧＨｚ ｉ７ｑｕａｄ−ｃｏｒｅプロセッサ（各コアで２スレッド及び全体で８スレッド）及び一体型ＧＰＵメモリを備えたラップトップコンピュータを使用することができる。更に、コンピュータシステムは、１又は２以上のタイプのＲＡＭのような１次又は動的メモリ１２０、及び１又は２以上のタイプのハードディスク又はフラッシュＲＯＭのような２次又はストレージメモリ１６０を有することができる。特定の計算モジュール（ＩＭＵモジュール１２２、視覚−慣性走行距離計測モジュール１２６、及びレーザ走査モジュール１３２）を上記に開示したが、そのようなモジュールが単に上述の機能を有する例示的モジュールにすぎず、非限定的であることを認識しなければならない。同様に、上記に開示したタイプのコンピュータデバイス１１０は、単にそのようなセンサと共に使用することができるコンピュータデバイスの１つのタイプの例にすぎず、かつ本明細書に開示する目的のためのものであり、決して限定ではない。

【0037】

図１に示すように、マッピングシステム１００は、粗対細方式（図２を参照）でモーションを順次に回復する個々の計算モジュールを含む計算モデルを組み込んでいる。ＩＭＵ１０２（ＩＭＵ予測モジュール１２２）からのモーション予測から始めて、視覚−慣性密接結合方法（視覚−慣性走行距離計測モジュール１２６）は、モーションを推定し、レーザポイントをローカルに記録する。走査適合方法（走査適合精緻化モジュール１３２）は、推定されたモーションを更に精緻化する。走査適合精緻化モジュール１３２は、ポイントクラウドデータ１６５を記録してマップ（ボクセルマップ１３４）を構築する。このマップは、任意的なナビゲーションシステム１３６の一部としてマッピングシステムによって使用することができる。ナビゲーションシステム１３６は、搭載コンピュータシステム、１次メモリ内の計算モジュールとして含めることができ、又は全く個別のシステムを構成することができることは認識することができる。

【0038】

各計算モジュールは、センサシステムの各々のうちの１つからのデータを処理することができることを認識することができる。従って、ＩＭＵ予測モジュール１２２は、ＩＭＵシステム１０２から得られたデータから粗マップを発生させ、視覚−慣性走行距離計測モジュール１２６は、カメラシステム１０４からの精緻化されたデータを処理し、走査適合精緻化モジュール１３２は、レーザスキャナ１０６及びモータ符号器１０９からの最細粒度解像度データを処理する。更に、細粒度解像度モジュールの各々は、粗粒度モジュールから提示されるデータを更に処理する。視覚−慣性走行距離計測モジュール１２６は、ＩＭＵ予測モジュール１２２から受信されてそれによって計算されたマッピングデータを精緻化する。同様に、走査適合精緻化モジュール１３２は、視覚−慣性走行距離計測モジュール１２６によって提示されたデータを更に処理する。上記に開示したように、センサシステムの各々は異なる速度でデータを収集する。一非限定例では、ＩＭＵ１０２は、約２００Ｈｚの速度でデータ収集を更新することができ、カメラ１０４は、約５０Ｈｚの速度でデータ収集を更新することができ、レーザスキャナ１０６は、約５Ｈｚの速度でデータ収集を更新することができる。これらの速度は制限ではなく、例えば、それぞれのセンサのデータ収集速度を反映することができる。粗粒度データは、細粒度データよりも速い速度で収集することができ、粗粒度データは、細粒度データよりも速い速度で処理することができることは認識することができる。データ収集の特定周波数値及び様々な計算モジュールによる処理を上記に開示したが、絶対的周波数もそれらの相対的周波数も限定ではない。

【0039】

マッピング及び／又はナビゲーショナルデータは、粗レベルデータ及び細レベルデータを含むと考えることができる。従って、１次メモリ（動的メモリ１２０）では、粗位置データは、計算モジュール１２２、１２６、１３２のいずれかによってアクセス可能にすることができるボクセルマップ１３４に格納することができる。走査適合精緻化モジュール１３２によって発生されるポイントクラウドデータ１６５としてのファイル詳細マッピングデータは、ハードドライブ、フラッシュドライブ、又は他の永続メモリのような２次メモリ１６０にプロセッサ１５０を通じて格納することができる。

【0040】

粗粒度データが細粒度計算のために計算モジュールによって使用されるだけでなく、視覚−慣性走行距離計測モジュール１２６及び走査適合精緻化モジュール１３２（細等級位置情報及びマッピング）は、ＩＭＵ位置予測を更新するための基礎として更に精緻化されたマッピングデータをそれぞれのフィードバック経路１２８及び１３８を通じてＩＭＵ予測モジュール１２２にフィードバックすることができる。この方式で、粗位置及びマッピングデータは、順次に解像度を精緻化することができ、精緻化された解像度データは、粗解像度計算のフィードバック基準として寄与する。

【0041】

図２は、３つの計算モジュールのそれぞれのデータ経路に沿ったブロック図を示している。ＩＭＵ予測モジュール１２２は、ＩＭＵ（図１の１０２）からＩＭＵ位置データ２２３を受信することができる。視覚−慣性走行距離計測モジュール１２６は、ＩＭＵ予測モジュール１２２からのモデルデータ、並びにカメラ（図１の１０４）からの１又は２以上の個々に追跡された視覚特徴２２７ａ、２２７ｂからの視覚データを受信することができる。レーザスキャナ（図１の１０６）は、視覚−慣性走行距離計測モジュール１２６によって供給される位置データに加えて走査適合精緻化モジュール１３２に供給されるレーザ決定式ランドマーク２３３ａ、２３３ｂに関するデータを発生させることができる。視覚−慣性走行距離計測モジュール１２６からの位置推定モデルは、フィードバック１２８されてＩＭＵ予測モジュール１２２によって計算された位置モデルを精緻化することができる。同様に、走査適合精緻化モジュール１３２からの精緻化マップデータは、フィードバック１３８されてＩＭＵ予測モジュール１２２によって計算された位置モデルに追加の補正を加えることができる。

【0042】

図２に示すようにかつ上記に開示したように、モジュール式マッピングシステムは、粗対細方式でモーション関連データを順次に回復及び精緻化することができる。更に、各モジュールのデータ処理は、モジュールにデータを供給するデバイスの各々のデータ収集及び処理速度によって決定することができる。ＩＭＵからのモーション予測から開始して、視覚−慣性密接結合方法は、モーションを推定してローカルにレーザポイントを記録する。次に、走査適合方法は、推定されたモーションを更に精緻化する。走査適合精緻化モジュールは、ポイントクラウドを記録してマップを構築することができる。その結果、マッピングシステムは、データが利用可能になる時に各精緻化フェーズを処理するように時間最適化される。

【0043】

図３は、図１に示す同じセンサタイプから得られたデータに基づく標準的カルマンフィルタモデルを示している。図３に示すように、カルマンフィルタモデルは、データの分解機能に関わらずセンサのいずれかからのあらゆるデータの受信に応答して位置及び／又はマッピングデータを更新する。従って、例えば、ＩＭＵデータに基づく位置情報推定値の状態に関わらずこれらのデータが利用可能になるいずれの時間にも視覚−慣性走行距離計測データを使用して位置情報を更新することができる。従って、カルマンフィルタモデルは、各タイプの測定の相対的な解像度を利用しない。図３は、位置データを最適化するための標準カルマンフィルタベースの方法のブロック図を示している。カルマンフィルタは、データが提示された時に順次に位置モデル３２２ａ−３２２ｎを更新する。従って、初期位置予測モデル３２２ａから開始して、カルマンフィルタは、ＩＭＵ機械化データ３２３の受信に基づいて精緻化することができる次の位置モデル３２２ｂを予測することができる３２４ａ。位置予測モデルは、個々の視覚特徴又はレーザランドマークでシードされた更新段階の前に予測段階３２４ａでＩＭＵ機械化データ３２３に応答して更新することができる３２２ｂ。

【0044】

図４は、因子−グラフ方法に基づく位置最適化を示している。この方法では、第１の時間４１０のモバイルマッピングシステムの姿勢をデータの受信に応答して第２の時間４２０の姿勢に更新することができる。因子−グラフ最適化モデルは、各精緻化計算中の全センサからの制約を組み合わせる。従って、カメラからのＩＭＵデータ３２３、特徴データ３２７ａ、３２７ｂなど、及びレーザランドマークデータ３３３ａ、３３３ｂなどは、全て各更新段階に使用される。そのような方法は、望ましい大量のデータによる各位置精緻化段階の計算の複雑さを増すことは認めることができる。センサは、大きさの程度によって異なる場合がある独立速度でデータを提供することができるので、全精緻化段階は、最も遅いセンサのデータ収集時間に時間的に制限される。その結果、そのようなモデルは高速リアルタイムマッピングに適切ではない場合がある。図１及び２に示すモジュール式システムは、粗対細方式におけるモーションを順次に回復する。この方式では、モーション精緻化の程度は、各タイプのデータの利用可能性によって決定される。

【0045】

仮定、座標、及び問題
仮定及び座標系
図１に示すように、モバイルマッピングシステムのセンサシステムは、レーザ１０６、カメラ１０４、及びＩＭＵ１０２を含むことができる。カメラは、固有のパラメータが公知であるピンホールカメラモデルとしてモデル化することができる。３つのセンサの全ての間の外因性パラメータは較正することができる。カメラとレーザ間の相対的姿勢及びレーザとＩＭＵ間の相対的姿勢は、当業技術で公知の方法に従って決定することができる。単一座標系をカメラ及びレーザに使用することができる。一非限定例では、カメラ座標系を使用することができ、全てのレーザポイントを前処理でカメラ座標系に投影することができる。一非限定例では、ＩＭＵ座標系は、カメラ座標系に平行にすることができ、従って、ＩＭＵ測定は、収集に応答して回転させながら補正することができる。座標系は、以下のように定義することができる。
・カメラ座標系｛Ｃ｝は、ｘ軸が左を指し、ｙ軸が上を指し、ｚ軸がカメラ主軸に一致する前方を指すカメラ光学中心から派生することができる。
・ＩＭＵ座標系｛Ｉ｝は、ｘ−、ｙ−、及びｚ−軸が｛ｃ｝に平行であり、同じ方向を指すＩＭ測定中心から派生することができる。
・世界座標系｛Ｗ｝は、開始姿勢の｛Ｃ｝に一致する座標系とすることができる。

【0046】

ＭＡＰ推定問題
状態推定問題は、機能的最大確率（ＭＡＰ）推定問題として定式化することができる。本発明者は、システム状態のセットＵ＝｛ｕｉ｝としてｘ＝｛ｘｉ｝、ｉ∈｛１；２；．．．，ｍ｝、制御入力のセットとしてｉ∈｛１；２；．．．，ｍ｝、及びランドマーク測定のセットとしてＺ＝｛ｚ_k｝、ｋ∈｛１；２；．．．，ｎ｝を定義することができる。提案するシステムが与えられる場合と、Ｚは、視覚特徴とレーザランドマークの両方を含むことができる。システムの複合確率は以下のように定義される。
式１

【0047】

Ｐ（ｘ₀）は、第１のシステム状態のプライアーであり、

は、モーションモデルを表し、

は、ランドマーク測定モデルを表している。（１）として定式化された各問題に対して、問題の対応するベイズ簡略ネットワーク表現がある。ＭＡＰ推定は式１を最大にすることである。ゼロ平均ガウスノイズの仮定の下で、問題は最小二乗問題と同等である。
式２

【0048】

ここで、

は、モーションモデル及びランドマーク測定モデルにそれぞれ関連付けられた残差である。

【0049】

式２を解く標準的な方法は、全てのセンサデータ、例えば、視覚特徴、レーザランドマーク、及びＩＭＵ測定値を大型因子−グラフ最適化問題に組み合わせることである。提案するデータ処理パイプラインは、代わりに、複数の小さい最適化問題を定式化して問題を粗対細方式で解く。最適化問題は以下のように言い直すことができる。
問題：レーザ、カメラ、及びＩＭＵから与えられたデータは、（２）として問題を定式化及び解いて、｛Ｗ｝に対して｛Ｃ｝の姿勢を決定し、次に、推定された姿勢を使用してレーザポイントを記録して｛Ｗ｝の横断された環境のマップを構築する。

【0050】

ＩＭＵ予測サブシステム
ＩＭＵ機械化
この小節ではＩＭＵ予測サブシステムを説明する。システムは基本的なセンサ座標系として｛Ｃ｝を考えているので、ＩＭＵは、｛Ｃ｝に対して特徴付けることができる。「仮定及び座標系」という名称の小節で上記に開示したように、｛Ｉ｝及び｛Ｃ｝は平行座標系であり、ω（ｔ）及びα（ｔ）は、時間ｔの｛Ｃ｝の角速度及び加速度それぞれを示す２つの３ｘ１ベクトルとすることができる。対応するバイアスは、ｂ_ω（ｔ）及びｂ_α（ｔ）として示すことができ、ｎ_ω（ｔ）及びｎ_α（ｔ）は対応するノイズである。ベクトル、バイアス、及びノイズの項は｛Ｃ｝で定義される。更に、ｇは、｛Ｗ｝の一定重力ベクトルとして示すことができる。ＩＭＵ測定項は次式の通りである。
式３

式４

ここで、

は、｛Ｗ｝から｛Ｃ｝への回転行列であり、

は、｛Ｃ｝及び｛Ｉ｝の間の変換ベクトルである。

【0051】

項

は、回転中心（｛Ｃ｝の始点）が｛Ｉ｝の始点とは異なるという事実による遠心力を表している。視覚−慣性ナビゲーション方法の一部の例は、この遠心力項を推定するために｛Ｉ｝のモーションをモデル化する。本明細書に開示する計算方法では、深度情報あり及びなしの視覚特徴が使用され、｛Ｃ｝から｛Ｉ｝の深度なしの変換特徴は直接的ではない（以下を参照）。その結果、本明細書に開示するシステムは、代わりに、｛Ｃ｝のモーションの全てをモデル化する。実際的にカメラ及びＩＭＵは、項の影響を最大限低減するために互いに近づけて装着される。

【0052】

ＩＭＵバイアスはゆっくり変化する変数とすることができる。この結果、最も新しく更新されたバイアスは、モーション積分に使用される。最初に、式３は時間で積分される。次に、得られる向きは、二度の時間積分のために式４と共に使用され、加速度データからの変換を取得する。

【0053】

バイアス補正
ＩＭＵバイアス補正は、カメラ又はレーザのいずれかからのフィードバックによって行うことができる（図１及び２の１２８、１３８をそれぞれ参照）。各フィードバック項は、短時間にわたる推定区分的モーションを含有する。バイアスは、区分的モーション中に一定になるようにモデル化することができる。式３から開始して、推定された向きをＩＭＵ積分と比較することによってｂ_ω（ｔ）を計算することができる。更新されたｂ_ω（ｔ）は、推定された変換と比較された変換を再度計算してｂ_α（ｔ）を計算するために更に一巡の積分に使用される。

【0054】

高周波数ノイズの影響を低減するために、バイアスの既知の数を維持するスライディングウィンドウが利用される。スライディングウィンドウに使用されるバイアスの数の非限定例は、２００ＨｚＩＭＵ速度に基づく４００バイアスの推奨される数を有する２００から１０００バイアスを含むことができる。１００ＨｚのＩＭＵ速度を有するスライディングウィンドウのバイアスの数の非限定例は、一般的に２００バイアスの値を有する１００から５００である。スライディングウィンドウからの平均化バイアスが使用される。この実施では、スライディングウィンドウの長さは、バイアスの更新速度を決定するためのパラメータとして機能する。バイアスをモデル化するための代替方法は当業技術で公知であるが、開示する実施は、個別のかつ異なるモジュールとしてＩＭＵ処理モジュールを維持するのに使用される。スライディングウィンドウ方法は、本発明のシステムの動的再構成を可能にすることができる。この方式では、ＩＭＵは、カメラ、レーザ、又は必要に応じてカメラとレーザの両方に結合することができる。例えば、カメラが機能しない場合に、ＩＭＵバイアスは、代わりに、レーザだけによって補正することができる。

【0055】

視覚−慣性走行距離計測サブシステム
視覚−慣性走行距離計測サブシステムのブロックシステム図を図５に示している。最適化モジュール５１０は、モーション推定５５０の深度情報を有するか又は持たない光学特徴データに基づいてカメラ制約５１５と共にＩＭＵ予測モジュール５２０からの姿勢制約５１２を使用する。深度マップ記録モジュール５４５は、レーザポイント５４０から得られた深度情報を有する追跡カメラ特徴５３０の深度マップ記録及び深度アソシエーションを含むことができる。深度マップ記録モジュール５４５は、前の計算から得られたモーション推定５５０を組み込むことができる。本方法は、視覚とＩＭＵを密接に結合する。各々は、区分的モーション５５０を推定する最適化モジュール５１０に制約５１２、５１５をそれぞれ提供する。同時に、本方法は、深度情報を深度マップ記録モジュール５４５の一部として視覚特徴に関連付ける。特徴が、レーザ範囲測定が利用可能な区域に位置付けられる場合に、深度がレーザポイントから得られる。そうでなければ、深度は、前に推定されたモーションシーケンスを使用して三角測量から計算される。最後のオプションとして、本方法は、異なる方法で制約を定式化することによっていずれの深度もない特徴を使用することができる。これは、レーザ範囲カバレージを持たないか又はカメラモーションの方向に十分長く追跡又は位置付けられないので三角測量できない特徴に対して真である。

【0056】

カメラ制約
視覚−慣性走行距離計測方法は、キー−フレームベースの方法である。新しいキー−フレームは、ある一定の数の特徴よりも多くが追跡を失うか又は画像オーバラップがある比率よりも下である場合に決定される５３５。従って、右の上付き文字ｌ、ｌ∈Ｚ⁺は最後のキー−フレームを示すことができ、ｃ、ｃ∈Ｚ⁺、及びｃ＞ｋは、現在のフレームを示すことができる。上記に開示したように、本方法は、深度あり及び深度なしで特徴を組み合わせる。キー−フレームｌの深度に関連付けられた特徴は、Ｘ₁＝｛Ｃ₁｝の［ｘ₁、ｙ₁、ｘ₁］^Tとして示すことができる。相応に、深度なしの特徴は、代わりに、標準化座標を使用して、

として示される。

及び

は、システム状態を表す式１の

とは異なる。キー−フレームの特徴は、２つの理由で深度に関連付けることができる：１）深度アソシエーションが処理の何らかの量を取り、キー−フレームのコンピュータ深度アソシエーションだけが計算強度を低減することができ、２）深度マップがフレームｃで利用できず、従って、記録が設定された深度マップに依存するのでレーザポイントを記録することができない。｛Ｃ_c｝の標準化特徴は、

として示すことができる。

【0057】

は、フレームl及びcの間の３ｘ３回転行列及び３ｘ１変換ベクトルとし、ここで、

変換を形成する。フレームｌとｃの間のモーション関数を次式のように書くことができる。
式５

【0058】

Ｘ_cは未知の深度を有する。ｄ_cを深度とし、ここで、

である。Ｘｃを

で置換して式５の第１及び第２の行と第３の行とを組み合わせてｄ_cを除き、次式をもたらす。
式６

式７

【0059】

の第ｈ行である。深度を特徴に利用できない場合に、ｄ₁をキー−フレームｌの未知の深度とする。Ｘ_l及びＸ_cを

それぞれで置換し、ｄ_k及びｄ_cを除くために式５の全ての３つの行を組み合わせて別の制約をもたらす。
式８

【0060】

モーション推定
モーション推定処理５１０は、１）式６−７の公知の深度を有する特徴から、２）式８の未知の深度を有する特徴から、及び３）ＩＭＵ予測５２０からの制約の３つのセットを組み合わせる最適化問題を解くのに必要である。

は、フレームａとｂの間の４ｘ４変換行列として定義することができる。
式９

【0061】

ここで、

は、対応する回転行列及び変換ベクトルである。更に、

に対応する３ｘ１ベクトルとすると、

は、回転角度である。各

のセットに対応する。

【0062】

フレームｌとｃ−１の間で解かれたモーション変換、すなわち、

を使用して、ＩＭＵ姿勢制約を定式化することができる。

として示される最後の２つのフレームｃ−１とｃの間の予測される変換は、ＩＭＵ機械化から取得することができる。フレームｃの予測変換は以下として計算される。
式１０

【0063】

は、

に対応する６−ＤＯＦモーションである。ＩＭＵ予測変換

は、向きに依存することは理解することができる。一例として、向きは、式４の回転行列を通じて重力ベクトルの予測

、従って、積分される加速度を決定することができる。

の関数として定式化することができ、

として書き直すことができる。ＩＭＵ予測モジュール１２２（図１及び２）によって提供される２００Ｈｚ姿勢、並びに視覚−慣性走行距離計測モジュール１２６（図１及び２）によって提供される５０Ｈｚ姿勢は、両方とも姿勢関数であることは理解することができる。

の計算は、フレームｃで開始することができ、加速度は、時間に関して逆積分することができる。

は解かれるモーションである。制約を次式として表すことができる。
式１１

ここで、

は、カメラ制約に対して姿勢制約を適切にスケーリングする相対的共分散行列である。

【0064】

視覚−慣性走行距離計測サブシステムでは、姿勢制約がモーションモデルを満足してカメラ制約が式２のランドマーク測定モデルを満足する。最適化問題は、異常値特徴除去のためのロバストな当て嵌めフレームワークに適応されたニュートン勾配−降下方法を使用することによって解くことができる。この問題では、状態空間が

を含有する。従って、フルスケールＭＡＰ推定は実行されないが、マージン化問題を解くためだけに使用される。ランドマーク位置は最適化されず、従って、状態空間の６つの未知数だけが使用され、それによって計算強度を低く維持する。従って、本発明の方法は、レーザ範囲測定を伴い、特徴に正確な深度情報を提供し、モーション推定の精度を保証する。その結果、バンドル精緻化を通じた特徴の深度の更に別の最適化は必要ない。

【0065】

深度アソシエーション
深度マップ記録モジュール５４５は、以前に推定されたモーションを使用して深度マップ上にレーザポイントを記録する。カメラの視野内のレーザポイント５４０は、ある一定の時間量に対して維持される。深度マップは一定密度を維持するためにダウンサンプリングされ、高速インデキシングのための２ＤのＫＤツリーに格納される。ＫＤツリーでは、全てのレーザポイントが、カメラ中心の周りの単位球面に投影される。ポイントはポイントの２つの角座標によって表される。深度を特徴に関連付ける時に、特徴を球面に投影することができる。３つの最も近いレーザポイントが各特徴に対して球面上に見出される。次に、それらの有効性は、直交空間の３つのポイント間の距離を計算することによって得られる。距離が閾値よりも大きい場合に、ポイントが、物体、例えば、壁及び壁の前面の物体とは異なる機会が高く、有効性検査が失敗する。最終的に、深度は、直交空間内にローカル平面パッチを有する３つのポイントから補間される。

【0066】

レーザ範囲カバレージのない特徴は、それらがある一定の距離にわたって追跡され、かつカメラモーションの方向に位置付けられない場合に、特徴が追跡される画像シーケンスを使用して三角測量することができる。そのような手順では、ベイズ確率モードに基づいて各フレームの深度を更新することができる。

【0067】

走査適合サブシステム
このサブシステムは、レーザ走査適合による前のモジュールからのモーション推定値を精緻化する。図６は、走査適合サブシステムのブロック図を示している。サブシステムは、提供された走行距離計測推定５５０を使用してローカルポイントクラウドでレーザポイント５４０を受信してこれらを記録する６２０。次に、図形特徴がポイントクラウドから検出され６４０、かつマップに適合される。走査適合は、当業技術で公知の多くの方法と同様に特徴対マップの距離を最小にする。しかし、走行距離計測推定５５０は、最適化６１０で姿勢制約６１２を提供する。最適化は、レーザ制約６１７によって発見及び更に処理される特徴対応性６１５を有する姿勢制約を処理する段階を含み、デバイス姿勢６５０を発生させる。この姿勢６５０は、特徴対応性６１５の発見を容易にするマップ記録処理６５５を通じて処理される。この実施はマップのボクセル表現を使用する。更に、それは、１から複数のＣＰＵスレッドに並行して実行されるように動的に構成することができる。

【0068】

レーザ制約
レーザ走査を受信する時に、本方法は、最初に走査６２０からのポイントを共通座標系に記録する。

は、走査数を示すのに使用することができる。カメラ座標系をカメラとレーザの両方に使用することができることは理解される。走査ｍは、

として示される走査の最初のカメラ座標系に関連付けることができる。視覚−慣性走行距離計測からレーザポイント５４０、走行距離計測推定５５０をローカルに記録６２０することは、キー−ポイントとして取ることができ、ＩＭＵ測定を使用してキー−ポイントの間を補間することができる。

【0069】

は、走査ｍからのローカルに記録されたポイントクラウドとする。Ｐ_mからの図形特徴の２つのセットを抽出することができ、一方はシャープエッジで、すなわち、エッジポイントで

として示され、他方はローカル平面表面で、すなわち、平面ポイントで

として示されている。これは、ローカル走査での曲率の計算を通じたものである。塞いだ領域の境界上のポイント及びローカル表面がレーザビームに平行になるように近いポイントのような近傍ポイントが既に選択されているポイントは避けられる。これらのポイントは、大きいノイズを含有するか又はセンサが移動する時に経時的に位置を変える場合がある。

【0070】

図形特徴は、構築された現在のマップに適合される。

は｛Ｗ｝で定義される。

のポイントは、エッジポイント及び平面ポイントをそれぞれ含有する２つのセットに分離される。ボクセルを使用して、センサの周りのある一定の距離で切り捨てられたマップを格納することができる。各ボクセルに対して、２つの３ＤのＫＤツリーを構築することができ、一方はエッジポイントに対して、片方は平面ポイントに対してのものである。個々のボクセルのＫＤツリーの使用は、クエリポイントが与えられているのでポイント検索を加速し、単一ボクセルに関連付けられた特定のＫＤツリーを検索する必要がある（以下を参照）。

【0071】

走査を適合させる時に、｛Ｗ｝への

が、最初に、利用可能なモーションの最良推測を使用して投影され、次に、

の各ポイントに対して、最も近いポイントのクラスターが、マップ上の対応するセットから見出される。ポイントクラスターの図形分布を検証するために、関連付けられた固有値及び固有ベクトルを調べることができる。特に、１つの大きい及び２つの小さい固有値がエッジラインセグメントを示し、２つの大きい及び１つの小さい固有値がローカル平面パッチを示している。適合が有効である場合に、式は、ポイントから対応するポイントクラスターまでの距離に関して定式化される。
式１２

ここで、

のポイントであり、

は、｛Ｗ｝での{C_m}の６−ＤＯＦ姿勢を示している。

【0072】

モーション推定
走査適合は、式１２によって説明した全体的な距離を最小にする最適化問題６１０に定式化される。最適化は、前のモーションからの姿勢制約６１２を含有する。

を{W}での

の姿勢に関する４×４変換行列とすると、

は、最後の走査を処理することによって発生される。

は、走行距離計測推定によって提供される{C_m-1}から{C_m}への姿勢変換とする。式１０と同様に、{W}での{C_m}の予測される姿勢変換は次式である。
式１３

【0073】

に対応する６−ＤＯＦ姿勢とし、

は、相対的共分散行列とする。制約は次式になる。
式１４

【0074】

式１４は、前のモーションが視覚−慣性走行距離計測からのものである場合を示し、カメラが機能していると仮定する。そうでなければ、制約はＩＭＵ予測からのものである。

を使用して、ＩＭＵ機械化による同じ項を示すことができる。加速度の積分が向き（式１１の

の関数である。ＩＭＵ姿勢制約は次式になる。
式１５

ここで、

は、対応する相対的共有結合行列である。最適化問題では、式１４及び１５は、制約の１つのセットに直線的に組み合わされる。線形組合せは、視覚−慣性走行距離計測の作動モードによって決定される。最適化問題は、ロバストな当て嵌めフレームワークに適用されるニュートン勾配−下降方法によって解かれる

【0075】

ボクセル内のマップ
マップ上のポイントはボクセルで維持される。Ａ２−レベルボクセル実施を図７Ａ及び７Ｂに示している。

は、最後の走査を処理した後の第１のレベルマップ７００上のボクセルのセット７０２、７０４を示している。センサ７０６の周りのボクセル７０４は、

を形成する。６−ＤＯＦセンサ姿勢、

が与えられた状態で、マップ上のセンサと共に移動する対応する

がある。センサがマップの境界に近づいた時は、境界の反対側７２５上のボクセルが、マップ境界７３０を拡張するために移動される。移動したボクセルのポイントは消え、マップの切り捨てをもたらす。

【0076】

図７Ｂに示すように、第２のレベルマップ７５０の各ボクセル、

は、第１のレベルマップ７００よりも大きさが小さい

として示されるボクセルのセットによって形成される。走査を適合させる前に、

のポイントは、モーションの最良推測を使用してマップに投影され、これを

に充填する。

からのポイントによって占められるボクセル７０８は、

を形成するために抽出され、走査適合のために３ＤのＫＤツリーに格納される。ボクセル７１０は、

からのポイントによって占められないものである。走査適合の完了に応答して、走査は、マップを備えたボクセル７０８に融合される。この後で、マップポイントは、ある一定の密度を維持するためにダウンサイジングされる。第１のレベルマップ７００の各ボクセルが第２のレベルマップ７５０のサブボクセルよりも大きい空間の容積に対応することは理解することができる。従って、第１のレベルマップ７００の各ボクセルは、第２のレベルマップ７５０の複数のサブボクセルを含み、第２のレベルマップ７５０の複数のサブボクセルにマップすることができる。

【0077】

図７Ａ及び７Ｂに関して上述したように、ボクセルの２つのレベル（第１のレベルマップ７００及び第２のレベルマップ７５０）は、マップ情報を格納するのに使用される。

に対応するボクセルが第１のレベルマップ７００を維持するのに使用され、第２のレベルマップ７５０の

に対応するボクセルは、走査適合のためのセンサの周りのマップを取り出すのに使用される。マップは、センサがマップ境界に近づく時だけ切り捨てられる。従って、センサがマップの内側をナビゲートする場合に、切り捨ては必要ない。別の考慮は、２つのＫＤツリーが

における各個々のボクセルに使用され、一方がエッジポイントに、他方が平面ポイントに使用される。上述のように、そのようなデータ構造は、ポイント検索を加速することができる。この方式では、複数のＫＤツリーの検索は、

で各個々のボクセルに対する２つのＫＤツリーの使用とは対照的に避けられる。後者は、ＫＤツリー構築及び保守に多くのリソースを必要とする。

【0078】

表１は、異なるボクセル及びＫＤツリー構成を使用するＣＰＵ処理時間を比較するものである。この時間は、閉じられた環境及び開かれた環境、構造物のある環境及び植生区域を含む異なるタイプの環境から収集された複数のデータセットから平均化される。ボクセルの１つのレベルのみ、

の使用が、ＫＤツリー構築及びクエリに約２倍の処理時間をもたらすことを見ることができる。これは、ボクセルの第２のレベル、

が、マップの正確な検索を補助するためである。これらのボクセルなしでは、より多くのポイントが

に含有され、ＫＤツリーに蓄積される。更に、各ボクセルに対してＫＤツリーを使用することにより、

で全てのボクセルに対してＫＤツリーを使用することに比べて僅かに処理時間が低減される。

【0079】

（表１）表１．ＫＤツリー作動における平均ＣＰＵ処理時間の比較

【0080】

並行処理
走査適合は、ＫＤツリーを構築する段階及び特徴対応性を繰返し見出す段階を伴う。この処理は、時間を消費し、システムの主要な計算を費やす。１つのＣＰＵスレッドが望ましい更新周波数を保証できない場合に、マルチスレッド実施が複雑な処理の問題に対処することができる。図８Ａは、２つの適合化器プログラム８１２、８１５が並行して実行される場合を示している。走査の受信に応答して、マネージャプログラム８１０は、これを利用可能な最新のマップに適合させるように並べる。複数の構造物及び複数の視覚特徴を有するクラスター化環境から構成される一例では、適合が遅く、次の走査の到着前に完了できない場合がある。２つの適合化器８１２及び８１５が代わりに呼び出される。１つの適合化器８１２では、

８１３ｎそれぞれに適合される。同様に、第２の適合化器８１５では、

（ｋ＝奇数の整数）８１６ｎそれぞれに適合される。このインターリーブ処理の使用は、処理のために２倍の時間量をもたらす場合がある。いくつかの構造物又は視覚特徴を備えた清浄な環境から構成される代替の例では、計算が軽い。そのような例（図８Ｂ）では、単一適合化器８２０だけを呼び出すことができる。インターリーブが必要ないので、

にそれぞれ適合される（８２７ａ、８２７ｂ、８２７ｎを参照）。実施は４つのスレッドの最大値を使用するように構成することができるが、一般的に２つのスレッドだけが必要になる。

【0081】

変換積分
最終モーション推定は、図２に示す３つのモジュールからの出力の積分である。５Ｈｚ走査適合出力は最も正確なマップを発生し、５０Ｈｚ視覚−慣性走行距離計測出力及び２００ＨｚＩＭＵ予測は、高周波数モーション推定のために積分される。

【0082】

ロバスト性に関して
システムのロバスト性は、センサ劣化を扱う機能によって決定される。ＩＭＵは、システムのバックボーンとしての信頼できる機能であると常に仮定される。カメラは、劇的照明変化の影響を受け易く、暗闇／テクスチャの少ない環境又は重大なモーションのぼけが存在する（それによって視覚特徴追跡の損失を引き起こす）時に失敗する場合がある。レーザは、構造物の少ない環境、例えば、単一平面によって支配されるシーンに対処することができない。これに代えて、レーザデータ劣化は、積極的なモーションによるデータの疎らさによって発生する場合がある。

【0083】

視覚−慣性走行距離計測及び走査適合モジュールの両方は、式２に従って最適化問題を定式化して解く。欠陥が引き起こされた時に、これは劣化最適化問題に相当し、すなわち、問題の一部の方向における制約は条件が悪く、ノイズが、解を決定する場合に支配的になる。１つの非限定的方法では、問題に関連付けられた

として示される固有ベクトルを計算することができる。センサの状態空間が６−ＤＯＦ（６の自由度）を含有するので６つの固有値／固有ベクトルが存在する。一般性を排除することなく、

は、減少する順序で並べ替えることができる。各固有値は、解がその対応する固有ベクトルの方向にどのように確実に条件付けられるかを説明する。固有値と閾値を比較することにより、確実に条件付けられた方向は、状態空間の劣化した方向から分離することができる。

を確実に条件付けされた方向の数とする。２つの行列を次式として定義することができる。
式１６

【0084】

最適化問題を解く時は、非線形反復を初期推測によって開始することができる。図２に示す順次パイプラインにより、ＩＭＵ予測は、出力が走査適合のための初期推測として取られる視覚−慣性走行距離計測のための初期推測を提供する。追加の２つのモジュール（視覚−慣性走行距離計測及び走査適合モジュール）に対して、ｘを解及びΔｘを非線形反復におけるｘの更新とし、Δｘは、線形化システム式を解くことによって計算される。最適化処理中に、全ての方向のｘを更新する代わりに、ｘを確実に条件付けられた方向でのみ更新することができ、代わりに劣化方向における初期推測を保持する。
式１７

【0085】

式１７では、システムは、粗対細順序でモーションを解き、ＩＭＵ予測から開始して、追加の２つのモジュールが、可能な限り多くのモーションを更に解く／精緻化する。問題が確実に条件付けられた場合に、精緻化は全ての６−ＤＯＦを含むことができる。そうでなければ、問題が部分的にのみ確実に条件付けられた場合に、精緻化は、０から５−ＤＯＦを含むことができる。問題が完全に劣化した場合に、

【0086】

図１４及び１５に示す姿勢制約に戻って、２つの式が走査適合問題に直線的に組み合わされたことを理解することができる。式１６に定義したように、

は、視覚−慣性走行距離計測モジュールからの固有ベクトルを含有する行列を示し、

以下の通りである。
式１８

【0087】

カメラが機能している標準的な場合では、

及び式１８は、式１４の視覚−慣性走行距離計測からの姿勢制約から構成される。しかし、カメラデータが完全に劣化した場合に、

姿勢制約から構成される。

【0088】

カメラ劣化の事例研究
図９Ａに示すように、視覚特徴が視覚−慣性走行距離計測に不十分にしか利用できない場合に、ＩＭＵ予測１２２は、視覚−慣性走行距離計測問題で確実に条件付けられた方向の数に応答して、視覚−慣性走行距離計測モジュール１２６を点線に示すように完全に又は部分的にバイパスする９２４。走査適合モジュール１３２は、走査適合のためのレーザポイントをローカルに記録することができる。ＩＭＵ予測のバイパスはドリフトの影響を受ける。レーザフィードバック１３８は、カメラフィードバック１２８が利用できない方向にのみＩＭＵの速度ドリフト及びバイアスを補正するカメラフィードバック１２８を補償する。従って、カメラフィードバックは、カメラデータが劣化しなかった時にこれを適合させる高周波数に起因してより高い優先度を有する。十分な視覚特徴が見出された時に、レーザフィードバックは使用されない。

【0089】

レーザ劣化の事例研究
図９Ｂに示すように、環境構造がモーション推定を精緻化するための走査適合１３２に対して不十分である場合に、視覚−慣性走行距離計測モジュール１２６出力は、点線に示すようにマップ９３０上のレーザポイントを記録するために走査適合モジュールを十分に又は部分的にバイパスする。確実に条件付けられた方向が走査適合問題に存在する場合に、レーザフィードバックは、これらの方向の精緻化されたモーション推定値を含有する。そうでなければ、レーザフィードバックが空白になる１３８。

【0090】

カメラ及びレーザ劣化の事例研究
更に複雑な例では、カメラとレーザの両方が少なくともある程度は劣化する。図１０はそのような例を示している。６つの行を有する垂直バーは６−ＤＯＦ姿勢を表し、各行は、式１６の固有ベクトルに対応するＤＯＦ（自由度）である。この例では、視覚−慣性走行距離計測及び走査適合の各々がモーションの３ＤＯＦを更新し、他の３−ＤＯＦのモーションは変化させない。ＩＭＵ予測１０２２ａ−ｆは初期ＩＭＵ予測値１００２を含むことができる。視覚−慣性走行距離計測は一部の３−ＤＯＦ（１０２６ｃ、１０２６ｅ、１０２６ｆ）を更新し１００４、精緻化済み予測１０２６ａ−１０２６ｆをもたらす。走査適合は、一部の３−ＤＯＦ（１０３２ｂ、１０３２ｄ、１０３２ｆ）を更新し１００６、更に別の精緻化済み予測１０３２ａ−１０３２ｆをもたらす。カメラフィードバック１２８はカメラ更新１０２８ａ−１０２８ｆを含有し、レーザフィードバック１３８はレーザ更新１０３８ａ−１０３８ｆをそれぞれ含有する。図１０を参照すると、陰影付けされていないセル（１０２８ａ、１０２８ｂ、１０２８ｄ、１０３８ａ、１０３８ｃ、１０３８ｅ）は、それぞれのモジュールからのいずれの更新情報も含有しない。ＩＭＵ予測モジュールへの合計更新１０８０ａ−１０８０ｆは、カメラフィードバック１２８からの更新１０２８ａ−１０２８ｆ及びレーザフィードバック１３８からの更新１０３８ａ−１０３８ｆの組合せである。フィードバックがカメラ（例えば、１０２８ｆ）及びレーザ（例えば、１０３８ｆ）の両方から利用可能な１又は２以上の自由度では、カメラ更新（例えば、１０２８ｆ）は、レーザ更新（例えば、１０３８ｆ）よりも優先度を有することができる。

【0091】

しかし、実際には、視覚−慣性走行距離計測モジュール及び走査適合モジュールは異なる周波数で実行する場合があり、各々がその固有の劣化方向を有する場合がある。ＩＭＵメッセージを使用して走査適合出力からの姿勢の間を補間することができる。この方式で、視覚−慣性走行距離計測出力に時間位置合わせされた区分的モーションを発生させることができる。

をフレームｃ−１とｃの間の視覚−慣性走行距離計測によって推定される６−ＤＯＦモーションとし、ここで

である。

を時間補間後の走査適合によって推定される対応する項とする。

は、視覚−慣性走行距離計測モジュールからの固有ベクトルを含有する式１６で定義された行列とすることができ、

劣化した方向を表している。

は、走査適合モジュールからの同じ行列とする。以下の式は組み合わされたフィードバック、

式１９

従って、

は、カメラ及びレーザフィードバックを表している。
式２０

式２１

【0092】

だけが状態空間のサブ空間に解かれたモーションを含有することに注意しなければならない。ＩＭＵ予測からのモーション、すなわち、

式２２

【0093】

を使用して、式３及び４の積分を通じて

の関数として定式化されたＩＭＵ予測モーションを示すことができる。向き

の両方に依存する。バイアスは、以下の式を解くことによって計算することができる。
式２３

【0094】

システムが正常に機能している時に、

は、ゼロ行列である。それに応じて、

から計算される。劣化した場合では、ＩＭＵ予測モーション、

は、モーションが解かれない方向に使用される（例えば、図１０の組み合わされたフィードバックの白い行１０８０ａ）。結果は、前に計算されたバイアスがこれらの方向で維持される。

【0095】

実験
スキャナによる試験
走行距離計測及びマッピングソフトウエアシステムは２つのセンサ組で検証される。第１のセンサ組では、ＶｅｌｏｄｙｎｅＬＩＤＡＲ（記録商標）ＨＤＬ−３２ＥレーザスキャナがＵＩ−１２２０ＳＥ白黒カメラ及びＸｓｅｎｓ（記録商標）ＭＴｉ−３０ＩＭＵに取り付けられる。レーザスキャナは、３６０°水平ＦＯＶ、４０°垂直ＦＯＶを有し、５Ｈｚスピン速度で７０万ポイント／秒を受信する。カメラは、７５２ｘ４８０ピクセルの解像度、７６°水平ＦＯＶ、及び５０Ｈｚフレーム速度から構成される。ＩＭＵ周波数は２００Ｈｚで設定される。第２のセンサ組では、ＶｅｌｏｄｙｎｅＬＩＤＡＲ（記録商標）ＶＬＰ−１６レーザスキャナが同じカメラ及びＩＭＵに取り付けられる。このレーザスキャナは、３６０°水平ＦＯＶ、３０°垂直ＦＯＶを有し、５Ｈｚスピン速度で３０万ポイント／秒を受信する。両方のセンサ組が、データ収集のために路上で及びオフロードの地形で運転される車両にそれぞれ取り付けられる。

【0096】

両方のセンサ組では、３００Ｈａｒｒｉｓコーナの最大値が追跡された。視覚特徴を均等に分配するために、画像は、５ｘ６の同一副領域に分割され、各副領域は最大１０特徴を提供する。特徴が追跡を失った時に、新しい特徴が各副領域の特徴数を維持するために発生される。

【0097】

本発明のソフトウエアは、ロボットオペレーティングシステム（ＲＯＳ）を実行するＬｉｎｕｘ（登録商標）システムで２．６ＧＨｚ ｉ７クアドコアプロセッサ（各コアに２スレッド及び全体で８スレッド）及び一体型ＧＰＵを有するラップトップコンピュータで実行される。ソフトウエアの２つのバージョンが、ＧＰＵ及びＣＰＵそれぞれで実行される視覚特徴追跡によって実施される。処理時間を表２に示している。視覚−慣性走行距離計測によって使用される時間（図２の１２６）は、環境又はセンサ構成に対してあまり変化しない。ＧＰＵバージョンでは５０Ｈｚで実行されるＣＰＵスレッドの約２５％を消費する。ＣＰＵバージョンではスレッドの約７５％を取る。センサの第１の組は、第２のセンサ組より僅かに多い処理時間をもたらす。これは、スキャナが多くのポイントを受信し、プログラムが深度マップを維持し、深度を視覚特徴に関連付けるのに多くの時間を必要とするからである。

【0098】

走査適合（図２の１３２）は、環境及びセンサ構成に関して変化する処理時間を多く消費する。第１のセンサ組では、構造物のある環境で作動された場合に走査適合は５Ｈｚで実行するスレッドの約７５％を取る。しかし、植物のある環境では、多くのポイントがマップに記録されプログラムは、一般的にスレッドの約１３５％を消費する。第２のセンサ組では、スキャナが受信するポイントが少ない。走査適合モジュール１３２は、環境に依存してスレッドの約５０−９５％を使用する。ＩＭＵ予測（図２の１３２）によって使用される時間は、他の２つのモジュールに比べて無視することができるものである。

【0099】

精度試験
提案するシステムの精度を認めるために試験が行われた。これらの試験では、第１のセンサ組が使用された。センサは、大学のキャンパスの周りで運転するオフロード車両に設けられた。１６分以内で２．７ｋｍ運転した後で、キャンパスマップが構築される。試験中の平均速度は２．８ｍ／ｓであった。

【0100】

（表２）表２．第１及び第２のセンサ組を使用した平均ＣＰＵ処理時間

【0101】

試験中のモーション推定ドリフトを評価するために、推定された軌跡及び記録されたレーザポイントが衛星画像上で位置合わせされた。従って、地面のレーザポイントが手動で取り除かれる。軌跡を衛星画像上で街路と適合させることにより、水平誤差の上限が＜１：０ｍであることが決定された。同じフロア上の建物を比較することにより、垂直誤差が＜２：０ｍであったことが決定された。これは最後には移動された距離の＜０．０９％になる全体的な相対的位置ドリフトを与える。精度が測定に対して保証されないことは理解することができ、従って、位置ドリフトの上限だけが計算された。

【0102】

乗客車両に設けられた同じセンサを有する更に包括的な試験が行われた。乗客車両は、構造物のある道路上を９．３ｋｍ移動して運転された。経路は、植生された環境、橋、丘の地形、及び交通量の多い街路を横断し、最後に開始位置に戻る。上昇は経路に沿って７０ｍ以上変化する。信号機を待つことを除いて、車両速度は、試験中に９−１８ｍ／ｓの間である。経路の始め及び終わりの両方で見出された建物が２つの中に記録されると決定された。２つの記録は、経路の長さにわたるモーション推定ドリフトのために発生する。従って、第１の記録は、試験の最初の車両に対応し、第２の記録は試験の終わりに車両に対応する。間隙が＜２０ｍであると測定され、移動した距離の＜０：２２％の最後の相対的位置誤差をもたらす。

【0103】

システムの各モジュールは全体的な精度に寄与する。図１１は精度試験の推定される軌跡を示している。視覚−慣性走行距離計測システムによって発生される移動センサの軌跡の第１の軌跡プロット１１０２は、ＩＭＵモジュール１２２及び視覚−慣性走行距離計測モジュール１２６を使用する（図２を参照）。第１の軌跡プロット１１０２に使用された構成は、図９Ｂに示すものに類似である。第２の軌跡プロット１１０４は、ＩＭＵモジュール１２２からのＩＭＵ予測を視覚−慣性走行距離計測をバイパスする走査適合モジュール１３２（図２を参照）への直接転送に基づいている。この構成は図９Ａに示すものに類似である。完全なパイプラインの第３の軌跡プロット１１０８は、ＩＭＵモジュール１２２、視覚−慣性走行距離計測モジュール１２６、及び走査適合モジュール１３２（図２を参照）の組合せに基づいて最小量のドリフトを有する。第１の２つの構成、軌跡プロット１１０２及び１１０４の位置誤差は、それぞれ約４及び２倍大きい。

【0104】

第１の軌跡プロット１１０２及び第２の軌跡プロット１１０４は、個々のセンサ劣化に遭遇した時に予測されるシステム性能として見ることができる。走査適合が劣化した場合に（図９Ｂを参照）、システムは、第１の軌跡プロット１１０２によって示されたモードに低下する。視野が劣化した場合に（図９Ａを参照）、システムは、第２の軌跡プロット１１０４に示すモードに低下する。センサのどれも劣化していない場合に（図２を参照）、システムは、軌跡プロット１１０８をもたらす最適化関数の全てを組み込んでいる。別の例では、システムは、初期推測としてＩＭＵ予測を取ることができるが、レーザ周波数（５Ｈｚ）で実行される。システムは、第４の軌跡プロット１１０６を発生させる。得られる精度は、視覚−慣性走行距離計測方法を通過するレーザに直接に結合されたＩＭＵを使用する第２の軌跡プロット１１０４と比較して僅かに良いだけである。結果は、互いにスタックされた全ての制約を有する問題を解く場合にカメラの機能が十分利用されないことを示している。

【0105】

システムの別の精度試験は、オリジナルの１ｘ速度及び加速された２ｘ速度で実行する移動センサを含む。２ｘ速度で実行された時に、全ての３つのセンサのあらゆる他のデータフレームが抑制され、試験を通じてより積極的なモーションをもたらす。結果を表３に列挙する。各速度で３つの構成が評価された。２ｘ速度では、視覚−慣性走行距離計測及びＩＭＵ＋走査適合構成の精度が、１ｘ速度の精度に比べて移動した距離の０．５４％及び０．３８％大幅に低減される。しかし、完全なパイプラインは、ごく僅かの０．０４％だけしか精度を低減しない。結果は、カメラ及びレーザが互いに補償し、全体的な精度を維持することを示している。これは特にモーションが積極的である時に真である。

【0106】

（表３）表３．移動した距離の百分率としての相対的位置誤差

【0107】

（図１１の軌跡に対応する１ｘ速度の誤差）

【0108】

図１２を参照すると、双方向情報フローの例示的かつ非限定的実施形態が示されている。例示するように、ＩＭＵ予測モジュール、視覚−慣性走行距離計測モジュール、及び走査−適合精緻化モジュールを含む３つのモジュールは、粗から細への段階的に問題を解く。データ処理フローは、左から右に３つのモジュールをそれぞれ通過し、フィードバックフローは右から左にＩＭＵのバイアスを補正する。

【0109】

図１３ａ及び１３ｂを参照すると、動的再構成可能システムの例示的かつ非限定的実施形態が示されている。図１３ａに示すように、視覚特徴が視覚−慣性走行距離計測に不十分である場合に、ＩＭＵ予測（部分的）が視覚−慣性走行距離計測モジュールをバイパスしてレーザポイントをローカルに記録する。一方では、図１３ｂに示すように環境構造が走査適合に不十分である場合に、視覚−慣性走行距離計測出力（部分的）は走査適合精緻化モジュールをバイパスしてマップにレーザポイントを記録する。

【0110】

図１４を参照すると、ＩＭＵバイアス補正のため優先度フィードバックの例示的かつ非限定的実施形態が示されている。例示するように、垂直バーは、６−ＤＯＦ姿勢を表し、各行がＤＯＦである。劣化した場合では、全ての６行が「ＩＭＵ」と示されている左のＩＭＵ予測から始めて、視覚−慣性走行距離計測方法が３−ＤＯＦを更新し、行の指定が「カメラ」になり、次に、走査適合が別の３−ＤＯＦを更新し、行の指定が「レーザ」に変わる。カメラ及びレーザフィードバックは左の垂直バーのように組み合わされる。カメラフィードバックは高優先度を有し、カメラフィードバックからの「カメラ」行が存在しない場合は、レーザフィードバックからの「レーザ」行だけが充填される。

【0111】

図１５ａ及び１５ｂを参照すると、マップの２層ボクセル表現の例示的かつ非限定的実施形態が示されている。マップ上のボクセルＭ_m-1（図１５ａの全てのボクセル）、及びセンサの周りのボクセルＳ_m-1（点で充填されたボクセル）が示されている。Ｓ_m-1は、Ｍ_m-1の部分集合である。センサがマップの境界に近づいた場合に、境界の反対側のボクセル（下の行）がマップ境界を延びるように移動する。移動したボクセル上のポイントはクリアされ、マップが切り捨てられる。図１５ｂに示すように、各ボクセル

（図１５ａの点で充填されたボクセル）が、大きさが小さいボクセルのセット

走査適合の前に、レーザ走査は、モーションの最良推測を使用してマップ上に投影される。

は、クロスハッチでラベル付けされる。次に、クロスハッチボクセル内のマップポイントが抽出され、走査適合のための３ＤのＫＤツリーに格納される。

【0112】

図１６を参照すると、走査適合のマルチスレッド処理の例示的かつ非限定的実施形態が示されている。例示するように、マネージャプログラムは、個別のＣＰＵスレッドで実行される複数の適合化器プログラムを呼び出し、走査を利用可能な最新のマップに適合させる。図１６ａは２スレッドの場合を示している。２倍の処理時間量が与えられると、走査

に適合される。比較すると、図１６ｂは１スレッドの場合を示し、従って、

に適合される。実施は、最大４つのスレッドを使用して動的に構成可能である。

【0113】

本発明の開示の一部の実施形態しか図示及び説明していないが、多くの変形及び修正が特許請求の範囲に示す本発明の開示の精神及び範囲から逸脱することなく本明細書に実行することができることは当業者に明らかであろう。外国及び国内両方の全ての特許出願及び特許、及び本明細書に引用のする全ての他の公開は、法律によって認められる限度まで全体が本明細書に組み込まれている。

【0114】

本明細書に説明する方法及びシステムは、コンピュータソフトウエア、プログラムコード、及び／又は命令をプロセッサで実行する機械を通じて部分的又は完全に配備することができる。本発明の開示は、機械における方法として、機械の一部又は機械に関したシステム又は装置として、又は機械の１又は２以上で実行されるコンピュータ可読媒体を用いて実施されるコンピュータプログラム製品として実施することができる。実施形態では、本発明のプロセッサは、サーバ、クラウドサーバ、クライアント、ネットワークインフラストラクチャ、移動コンピュータプラットフォーム、固定コンピュータプラットフォーム、又は他のコンピュータプラットフォームの一部とすることができる。プロセッサは、プログラム命令、コード、バイナリ命令などを実行することができる計算又は処理デバイスのいずれかの種類とすることができる。プロセッサは、信号プロセッサ、デジタルプロセッサ、組み込み式プロセッサ、マイクロプロセッサ、又はコプロセッサのようなあらゆる変形（数学コプロセッサ、グラフィックコプロセッサ、通信コプロセッサなど）及び格納しているプログラムコード又はプログラム命令の実行を直接又は間接的に容易にすることができるものとすることができ、又はこれらを含むことができる。更に、プロセッサは、複数のプログラム、スレッド、及びコードの実行を可能にすることができる。スレッドは、プロセッサの性能を拡張して用途の同時作動を容易にするために同時に実行することができる。実施により、本明細書に説明する方法、プログラムコード、プログラム命令などを１又は２以上のレッドを用いて実施することができる。スレッドは、これらに関連付けられた優先度を割り当てることができる他のスレッドを発生させることができ、プロセッサは、優先度又はプログラムコードで提供される命令に基づくあらゆる他の順序に基づいてこれらのスレッドを実行することができる。プロセッサ、又はプロセッサを利用するあらゆる機械は、本明細書及び他に説明する方法、コード、命令及びプログラムを格納する非一時的メモリを含むことができる。プロセッサは、本明細書及び他で説明する方法、コード、及び命令を格納することができる非一時的ストレージ媒体にインタフェースを通じてアクセス可能である。方法、プログラム、コード、プログラム命令、又はコンピュータ又は処理デバイスによって実行することができる他のタイプの命令を格納するためのプロセッサに関連付けられたストレージ媒体は、限定されないが、ＣＤ−ＲＯＭ、ＤＶＤ、メモリ、ハードディスク、フラッシュドライブ、ＲＡＭ、ＲＯＭ、キャッシュなどのうちの１又は２以上を含むことができる。

【0115】

プロセッサは、マルチプロセッサの速度及び性能を拡張することができる１又は２以上のコアを含むことができる。実施形態では、処理は、２又は３以上の独立コア（ダイと呼ぶ）を組み合わせるデュアルコアプロセッサ、クアドコアプロセッサ、他のチップレベルマルチプロセッサなどとすることができる。

【0116】

本明細書に説明する方法及びシステムは、サーバ、クライアント、ファイヤウォール、ゲートウェイ、ハブ、ルータ、又は他のそのようなコンピュータ及び／又はネットワーキングハードウエアでコンピュータソフトウエアを実行する機械を通じて一部又は全てを配備することができる。ソフトウエアプログラムは、ファイルサーバ、プリントサーバ、ドメインサーバ、インターネットサーバ、イントラネットサーバ、クラウドサーバ、及び２次サーバ、ホストサーバ、分散サーバのような他の変形を含むことができるサーバに関連付けることができる。サーバは、メモリ、プロセッサ、コンピュータ可読媒体、ストレージ媒体、ポート（物理的及び仮想的）、通信デバイスなどのサーバ、クライアント、機械、及びデバイスに、有線又は無線媒体などを通じてアクセス可能なインタフェースの１又は２以上を含むことができる。本明細書及び他で説明する方法、プログラム、又はコードは、サーバによって実行することができる。更に、この出願で説明する方法の実行に必要な他のデバイスは、サーバに関連付けられたインフラストラクチャの一部として考えることができる。

【0117】

サーバは、以下に限定されるものではないが、クライアント、他のサーバ、プリンタ、データベースサーバ、プリントサーバ、ファイルサーバ、通信サーバ、分散サーバ、ソーシャルネットワークなどを含む他のデバイスへのインタフェースを提供することができる。これに加えて、この結合及び／又は接続は、ネットワークを通じてプログラムのリモート実行を容易にすることができる。これらのデバイスの一部又は全てのネットワーキングは、本発明の開示の範囲から逸脱することなく１又は２以上の位置でプログラム又は方法の並行処理を容易にすることができる。更に、インタフェースを通じてサーバに接続されたデバイスのいずれも、方法、プログラム、コード、及び／又は命令を格納することができる少なくとも１つのストレージ媒体を含むことができる。中央リポジトリは、異なるデバイスで実行されるプログラム命令を提供することができる。この実施では、リモートリポジトリは、プログラムコード、命令、及びプログラムのためのストレージ媒体として作用することができる。

【0118】

ソフトウエアプログラムは、ファイルクライアント、プリントクライアント、ドメインクライアント、インターネットクライアント、イントラネットクライアント及び２次クライアント、ホストクライアント、分散クライアントのような他の変形を含むことができるクライアントに関連付けることができる。クライアントは、メモリ、プロセッサ、コンピュータ可読媒体、ストレージ媒体、ポート（物理的及び仮想的）、通信デバイスなどのクライアント、サーバ、機械、及びデバイスに有線又は無線媒体などを通じてアクセス可能なインタフェースの１又は２以上を含むことができる。本明細書及び他で説明する方法、プログラム、又はコードは、クライアントによって実行することができる。更に、この出願で説明する方法の実行に必要な他のデバイスは、クライアントに関連付けられたインフラストラクチャの一部として考えることができる。

【0119】

クライアントは、以下に限定されるものではないが、サーバ、他のクライアント、プリンタ、データベースサーバ、プリントサーバ、ファイルサーバ、通信サーバ、分散サーバなどを含む他のデバイスへのインタフェースを提供することができる。これに加えて、この結合及び／又は接続は、ネットワーク全体のプログラムのリモート実行を容易にすることができる。これらのデバイスの一部又は全てのネットワーキングは、本発明の開示の範囲から逸脱することなく１又は２以上の位置でのプログラム又は方法の並行処理を容易にすることができる。更に、インタフェースを通じてクライアントに接続されたデバイスのいずれも、方法、プログラム、用途、コード及び／又は命令を格納することができる少なくとも１つのストレージ媒体を含むことができる。中央リポジトリは、異なるデバイスで実行されるプログラム命令を提供することができる。この実施では、リモートリポジトリは、プログラムコード、命令、及びプログラムのためのストレージ媒体として作用することができる。

【0120】

本明細書に説明する方法及びシステムは、ネットワークインフラストラクチャを通じて一部又は全てを配備することができる。このネットワークインフラストラクチャは、コンピュータデバイス、サーバ、ルータ、ハブ、ファイヤウォール、クライアント、パーソナルコンピュータ、通信デバイス、経路指定デバイス、及び他のアクティブ及びパッシブデバイス、モジュール、及び／又は当業技術で公知の構成要素のような要素を含むことができる。ネットワークインフラストラクチャに関連付けられたコンピュータ及び／又は非コンピュータデバイスは、他の構成要素以外に、フラッシュメモリ、バッファ、スタック、ＲＡＭ、ＲＯＭのようなストレージ媒体を含むことができる。本明細書及び他で説明する処理、方法、プログラムコード、命令は、ネットワークインフラストラクチャ要素の１又は２以上によって実行することができる。本明細書に説明する方法及びシステムは、ソフトウエア・アズ・ア・サービス（ＳａａＳ）、プラットフォーム・アズ・ア・サービス（ＰａａＳ）、及び／又はインフラストラクチャ・アズ・ア・サービス（ＩａａＳ）の特徴を含有する特徴を含むプライベート、コミュニティ、又はハイブリッドクラウドコンピュータネットワーク又はクラウドコンピュータ環境のあらゆる種類と共に使用するように適応させることができる。

【0121】

本明細書及び他で説明する方法、プログラム、コード、及び命令は、セルラーネットワークｈａａ送信者制御式接触媒体コンテンツ項目マルチセル上で実施することができる。セルラーネットワークは、周波数分割多元接続（ＦＤＭＡ）ネットワーク又は符号分割多元接続（ＣＤＭＡ）ネットワークのいずれかとすることができる。セルラーネットワークは、モバイルデバイス、セルサイト、基地局、リピータ、アンテナ、タワーなどを含むことができる。セルネットワークは、ＧＳＭ(登録商標)、ＧＰＲＳ、３Ｇ、ＥＶＤＯ、メッシュ、又は他のネットワークタイプとすることができる。

【0122】

本明細書及び他で説明する方法、プログラムコード、及び命令は、モバイルデバイスで又はモバイルデバイスを通じて実施することができる。モバイルデバイスは、ナビゲーションデバイス、携帯電話、移動電話、移動携帯情報端末、ラップトップ、パームトップ、ネットブック、ポケットベル、電子書籍読取器、音楽プレーヤなどを含むことができる。これらのデバイスは、他の構成要素以外にフラッシュメモリ、バッファ、ＲＡＭ、ＲＯＭ、及び１又は２以上のコンピュータデバイスのようなストレージ媒体を含むことができる。モバイルデバイスに関連付けられたコンピュータデバイスは、格納しているプログラムコード、方法、及び命令を実行することができる。これに代えて、モバイルデバイスは、他のデバイスと協働して命令を実行するように構成することができる。モバイルデバイスは、サーバに接続されてプログラムコードを実行するように構成された基地局と通信することができる。モバイルデバイスは、ピア・ツー・ピアネットワーク、メッシュネットワーク、又は他の通信ネットワーク上で通信することができる。プログラムコードは、サーバに関連付けられたストレージ媒体に格納されてサーバ内に組み込まれたコンピュータデバイスによって実行することができる。基地局は、コンピュータデバイス及びストレージ媒体を含むことができる。ストレージデバイスは、基地局に関連付けられたコンピュータデバイスによって実行されるプログラムコード及び命令を格納することができる。

【0123】

コンピュータソフトウエア、プログラムコード、及び／又は命令は、コンピュータ構成要素、デバイス、及び何らかの時間間隔にわたって計算するのに使用されるデジタルデータを保存する記録媒体、ランダムアクセスメモリ（ＲＡＭ）として公知の固体ストレージ、一般的に光学ディスク、ハードディスク、テープ、ドラム、カード、及び他のタイプのような磁気ストレージの形態のような恒久的ストレージのための大容量ストレージ、プロセッサレジスタ、キャッシュメモリ、揮発性メモリ、不揮発性メモリ、ＣＤ、ＤＶＤのような光学ストレージ、フラッシュメモリ（例えば、ＵＳＢスティック又はキー）、フロッピーディスク、磁気テープ、紙テープ、パンチカード、独立ＲＡＭディスク、Ｚｉｐドライブ、着脱可能大容量ストレージ、オフラインのような着脱可能媒体、動的メモリ、静的メモリ、読取／書込ストレージ、変異ストレージ、読取専用、ランダムアクセス、シーケンシャルアクセス、位置アドレス可能、ファイルアドレス可能、コンテンツアドレスア可能、ネットワーク付属ストレージ、ストレージエリアネットワーク、バーコード、磁気リンクのような他のコンピュータメモリを含むことができる機械可読媒体上に格納される及び／又はそこにアクセス可能である。

【0124】

本明細書に説明する方法及びシステムは、物理的及び／又は非有形品目を１つの状態から別の状態に変形することができる。本明細書に説明する方法及びシステムはまた、物理的及び／又は非有形品目を表すデータを１つの状態から別の状態に変形することができる。

【0125】

本明細書に説明して図示する流れ図及び図面全体のブロック図に含む要素は要素間の論理的境界を意味する。しかし、ソフトウエア又はハードウエア工学の慣例により、図示の要素及びその機能は、コンピュータ実行可能媒体ｈａａ送信者制御式接触媒体コンテンツ項目を通じて機械を用いて実施することができ、プロセッサは、モノリシックソフトウエア構造として、独立ソフトウエアモジュールとして、又は外部ルーチン、コード、サービスなどを利用するモジュールとして、又はこれらのあらゆる組合せとして格納されたプログラム命令を実行することができ、全てのそのような実施は本発明の開示の範囲にあるものとすることができる。そのような機械の例は、限定されないが、携帯情報端末、ラップトップ、パーソナルコンピュータ、移動電話、他の手持ち式コンピュータデバイス、医療機器、有線又は無線通信デバイス、変換器、チップ、計算機、衛星、タブレットＰＣ、電子書籍、ガジェット、電子デバイス、デバイスｈａａ送信者制御式接触媒体コンテンツ項目人工知能、コンピュータデバイス、ネットワーキング機器、サーバ、ルータなどを含むことができる。流れ図及びブロック図に示す要素又はあらゆる他の論理的構成要素は、プログラム命令を実行することができる機械を用いて実施することができる。従って、上述の図面及び説明は開示したシステムの機能的態様を示し、これらの機能的態様を実施するためのソフトウエアの特定の構成は、明示的に記載するか又は他に関連から明らかでない限り、これらの説明からは推測されない。同様に、上記に識別及び説明した様々な段階を変更することができること、及び段階の順序を本明細書に開示する技術の特定の応用に適応することができることは認められるであろう。全てのそのような変形及び修正は、本発明の開示の範囲に入るものとする。同様に、様々な段階の順序の図示及び／又は説明は、特定の応用によって要求される又は明示的に記載する又は他に関連から明らかでない限り、これらの段階を実行する特定の順序を必要とすると理解してはならない。

【0126】

上述の方法及び／又は処理、及び本明細書に関連付けられた段階は、特定の応用に適するハードウエア、ソフトウエア、又はハードウエアとソフトウエアのあらゆる組合せで達成することができる。ハードウエアは、汎用コンピュータ及び／又は専用コンピュータデバイス又は特定のコンピュータデバイス又は特定のコンピュータデバイスの特定の態様又は構成要素を含むことができる。処理は、内部及び／又は外部メモリと共に１又は２以上のマイクロプロセッサ、マイクロコントローラ、組み込み式マイクロコントローラ、プログラマブルデジタル信号プロセッサ、又は他のプログラマブルデバイスで達成することができる。処理は、特定用途向け集積回路、プログラマブルゲートアレイ、プログラマブルアレイ論理部、又は電子信号を処理するように構成することができるあらゆる他のデバイス又はデバイスの組合せとすることができ、又はこれらを用いて実施することができる。処理の１又は２以上は、機械可読媒体で実行することができるコンピュータ実行可能コードとして達成することができることが更に認められるであろう。

【0127】

コンピュータ実行可能コードは、上述のデバイスの１つ、並びにプロセッサの異種組合せ、プロセッサアーキテクチャ、又は異なるハードウエア及びソフトウエアの組合せ、又はプログラム命令を実行することができるあらゆる他の機械の１つで実行するために格納、コンパイル、又は解釈することができるＣのような構造化プログラミング言語、Ｃ＋＋のようなオブジェクト指向プログラミング言語、又はあらゆる他の高レベル又は低レベルプログラミング言語（アセンブリ言語、ハードウエア記述言語、及びデータベースプログラミング言語及び技術を含む）を使用して作成することができる。

【0128】

すなわち、一態様では、上述の方法及びその組合せは、１又は２以上のコンピュータデバイスで実行された時にこれらの段階を実行するコンピュータ実行可能コードを用いて実施することができる。別の態様では、本方法は、この段階を実行するシステムを用いて実施することができ、いくつかの方法でデバイスにわたって分散させることができ、又は機能の全てを専用、独立デバイス、又は他のハードウエアに統合することができる。別の態様では、上述の処理に関連付けられた段階を実行するための手段は、上述のハードウエア及び／又はソフトウエアのいずれかを含むことができる。全てのそのような置換及び組合せは、本発明の開示の範囲に入るものとする。

【0129】

本発明の開示は、図示して詳細に説明した好ましい実施形態に関して開示したものであり、本発明の開示の様々な修正及び改良は、当業者には容易に明らかになるであろう。従って、本発明の開示の精神及び範囲は、上述の例によって制限されず、むしろ法律によって許容可能な様々な意味で理解されるものとする。

【0130】

本発明の開示を説明する関連（特に、特許請求の範囲の関連）における「ａ」及び「ａｎ」及び「ｔｈｅ」という語及び類似の指示語は、本明細書に他に指示がないか又は関連によって明確に矛盾しない限り、単数形及び複数形の両方を含むものと解釈しなければならない。「ｃｏｍｐｒｉｓｉｎｇ」、「ｈａａ送信者制御式接触媒体コンテンツ項目」、「ｉｎｃｌｕｄｉｎｇ」、及び「ｃｏｔａｉｎｉｎｇ」は、他に示されない限り、非限定的語として解釈されるものとする（すなわち、「限定されないが含む」を意味する）。本明細書の値の範囲の列挙は、本明細書に特に示さない限り値内に入る各それぞれの値を個々に指す簡単な方法として機能するものとし、個別の各値は、本明細書に個々に説明されているように本明細書に組み込まれる。本明細書に説明する全ての方法は、本明細書で特に示さない限り又は関連によって明確に矛盾しない限り、あらゆる適切な順序で実行することができる。本明細書に提供するいずれか及び全ての例、又は例示的言語（例えば、「のような」）の使用は、単に本発明の開示を的確に説明するものであり、他に主張されない限り、本発明の開示の範囲に制限を設けることはない。本明細書の言語は、本発明の開示の実施に不可欠ないずれかの主張されない要素を示すものとして解釈すべきではない。

【0131】

以上の説明は、当業者が現時点で最良のモードと考えられるものを使用することを可能にするものであり、当業者は、本明細書の特定の実施形態、方法、及び例の変形、組合せ、及び均等物の存在を理解かつ認めるであろう。従って、本発明の開示は、上述の実施形態、方法、及び例によって制限すべきではなく、むしろ本発明の開示の精神及び範囲内の全ての実施形態及び方法によって制限されるべきである。

【0132】

本明細書で参照した全ての文書は、これにより引用によって組み込まれる。

【符号の説明】

【0133】

ＩＭＵ 慣性測定ユニット

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7A】

【図7B】

【図8A】

【図8B】

【図9A】

【図9B】

【図10】

【図11】

【図12】

【図13a】

【図13b】

【図14】

【図15a】

【図15b】

【図16a】

【図16b】