JP2018156660A - 回転翼型小型飛行体（ｍａｖ）での屋内環境及び屋外環境における安定自律飛行のためのマルチセンサの融合 - Google Patents

Abstract

【課題】回転翼型小型飛行体（ＭＡＶ）の屋内環境および屋外環境での安定した自立飛行が可能なシステムを提供する。【解決手段】互いに異なり、変化する時間間隔で、絶対または相対観察値を発生させる複数の異種センサである、ＩＭＵ１１２、レーザスキャナ１１８、ステレオカメラ１２２、圧力高度計１２３、磁力計１１６およびＧＰＳレシーバ１２０からの、ノイズを含んだ測定値をプロセッサ１０８で統合し、自律飛行のためにリアルタイムにて、スムーズでグローバルに一貫した想定値を提供し、また、モジュール式で拡張が可能なアプローチを含んでいる。【選択図】図１２

Description

優先権主張
本願は２０１３年１１月２７日に出願された米国仮特許願第６１／９１００２２号の優先権を主張する。

政府権益
本発明は、海軍研究所によって付与された認可番号Ｎ０００１４−０７−１−０８２９、Ｎ０００１４−０８−１−０６９６、Ｎ０００１４−０９−１−１０３１およびＮ０００１４−０９−１−１０５１、全米科学財団によって付与された認可番号１１３８８４７、１１３８３０および１１３８１１０、空軍科学研究所により付与された認可番号Ｗ９１１ＮＦ−０８−２−０００４およびＷ９１１ＮＦ−１０−２−００１６、並びに陸軍研究所によって付与された認可番号ＦＡ９５５０−１０−１−０５６７の下で政府の支援によって為された。

ここで説明する主題は小型飛行体における自律飛行を制御することに関する。特に、ここで説明する主題は、屋内環境および屋外環境における回転翼型小型飛行体（ＭＡＶ）の安定自律飛行のためのマルチセンサの融合に関する。

小型飛行体、例えば回転翼型小型飛行体は自律的に飛行することができる。自律飛行のための制御入力の提供に利用できる十分なセンサデータが存在すれば正確な自律飛行が達成できる。例えば、全地球測位システム（ＧＰＳ）が利用できる屋外環境では、自律飛行はＧＰＳ信号に基づいて達成できる。しかしながらＧＰＳが利用できない環境では、例えば、屋内環境や、屋外の都市部環境においてさえもＧＰＳのみに基づく自律飛行は可能ではない。屋内環境によっては、構造物によって引き起こされる磁気障害により磁力計の出力が得られなかったり、信頼できなかったりする。よって、回転翼型ＭＡＶの飛行を制御するために単一モードのセンサに依存することは望ましくないであろう。

回転翼型ＭＡＶの自律飛行を制御する別な目的は、それまでは利用できなかったセンサモードの利用が可能になる様相／状態（以降“ステート”）間の移行をスムーズにすることである。例えば、回転翼型ＭＡＶが、ＧＰＳが利用できない屋内を飛行しているとき、ＧＰＳが急に利用可能になる屋外環境へ移行する際に、その回転翼体が大きく飛行コースから外れていると判断し、直ちに正規の飛行コースに戻す補正を試みるかも知れない。そのような移行は、直ちに飛行コースに戻すために回転翼体の飛行速度や飛行経路を大きく変えるものではなく、スムーズであることが望ましい。

回転翼型小型飛行体の自律飛行を制御するのに複数種のセンサデータを活用することができる。例えば、搭載カメラ、レーザスキャナ、ＧＰＳトランシーバ、および加速度計は飛行を制御するための制御入力として適している複数の入力を提供できる。しかし上述したように、これらセンサの１つに依存すると、そのセンサが関与する想定（または仮定）が機能しないときには、飛行は失敗することになる。それぞれのタイプのセンサは、その測定値が独自レベルの不確実性を含んだ独自の出力を発生させるので、回転翼型ＭＡＶの屋内環境および屋内環境での安定した自律飛行のためには、マルチセンサを融合させる改善された方法、システムおよびコンピュータ読取可能媒体が必要である。

ここで説明されている主題は、異なっており、且つ変動する時間間隔で絶対的観察または相対的観察を提供する複数の異種センサからのノイズを含んだ測定値を集積一体化し、自律飛行のための滑らかで全地球的（グローバル）に一貫（安定）した位置のリアルタイムでの推定（値）を提供するモジュール形態であって拡張が可能なアプローチ（取り組み）を含んでいる。我々（発明者）はここで、慣性測定装置（ＩＭＵ）、レーザスキャナ、ステレオカメラ、圧力高度計、磁力計、およびＧＰＳレシーバ（受信機）を備えた新式の１．９ｋｇのＭＡＶプラットフォームのためのアルゴリズムおよびソフトウェアアーキテクチャの開発を説明する。そこでは、ステートの推定および制御がインテルＮＵＣ第３世代ｉ３プロセッサによって機上で実行される。我々は、建物内に進入および建物内から脱出する際の風速が約１０ｍｐｈ（マイル／時）である条件下、１．５ｍ／秒の平均速度で４４０メートル以上を飛行する大きなスケールでの屋内−屋外自律飛行ナビゲーション実験により我々のフレームワーク（基本理念）の安定性（確実性）を説明する。

ここで説明する主題は、ハードウェア、ソフトウェア、ファームウェア、あるいはそれらの任意の組み合わせによって実現できる。よって、ここで使用される“ファンクション”、“ノード”または“モジュール”なる用語はハードウェアに言及するものであり、ここで説明している特徴を実行するためのソフトウェア及び／又はファームウェアのコンポーネントも含むことができる。１例示的実施態様においては、ここで説明する主題は、コンピュータのプロセッサで実行されると、プロセスステップを実施するようにコンピュータを制御するコンピュータ実行可能インストラクションを保存した、コンピュータ読取可能媒体を使用して実現できる。ここで説明する主題を実現するのに適した例示的コンピュータ読取可能媒体には、ディスクメモリ装置、チップメモリ装置、プログラム可能ロジック装置、およびアプリケーション特化集積回路などの非一時的コンピュータ読取可能媒体が含まれる。さらに、ここで説明する主題を実現するコンピュータ読取可能媒体は、単一装置またはコンピュータプラットフォームに配置することも、複数の装置あるいは複数のコンピュータプラットフォームに配分されることもできる。

ここで説明する主題を添付の図面に基づいて説明する。
ＩＭＵ、レーザスキャナ、ステレオカメラ、圧力高度計、磁力計およびＧＰＳレシーバ（GPS受信機）を備えた１．９ｋｇＭＡＶプラットフォームを図示する。全てのコンピュータ処理は第３世代ｉ３プロセッサを備えたインテルＮＵＣコンピュータによって機上にて実施された。 プライオリティキュー（優先順位付き待ち行列）を備えた遅延し、順序が狂った測定値を示す。ｚ_２の前にｚ_４が到着するが、ｚ_２がまずフィルタに適用された。ｚ_４はキューに一時的に保存された。ｚ_１は破棄された。なぜなら、それは現在ステートのｔｄよりも古いからである。共分散は、最新の測定値がフィルタに適用されるときまでプロパゲーションされるだけである。ステートは最新のＩＭＵ入力までプロパゲーションされた。 ＧＰＳ信号がｋ＝５で再取得され、測定値ｚ_５とステートｓ_５（図３（ａ））との間に大きな不一致が生じていることを図示する。ポーズ（空間位置／姿勢）グラフＳＬＡＭはグローバル（全地球的）に一貫したグラフ（図３（ｂ））を発生させた。 ＧＰＳ信号がｋ＝５で再取得され、測定値ｚ_５とステートｓ_５（図３（ａ））との間に大きな不一致が生じていることを図示する。ポーズ（空間位置／姿勢）グラフＳＬＡＭはグローバル（全地球的）に一貫したグラフ（図３（ｂ））を発生させた。 代替ＧＰＳ融合を示しており、変換されたＧＰＳ測定値ｚ_５と非最良ステートｓ_５との間の不一致は最小化されている。そのような間接ＧＰＳ測定の融合はスムーズ（円滑、滑らか）なステート推定値（ｓ_６とｓ_５の間の破線）に導く。 代替ＧＰＳ融合を示しており、変換されたＧＰＳ測定値ｚ_５と非最良ステートｓ_５との間の不一致は最小化されている。そのような間接ＧＰＳ測定の融合はスムーズ（円滑、滑らか）なステート推定値（ｓ_６とｓ_５の間の破線）に導く。 ＭＡＶが最大速度３．５ｍ／秒で積極的に飛行する様子を示す（図５（ｂ））。水平位置は多少のドリフトを伴ったグランドツルースに類似する（図５（ａ））。 ＭＡＶが最大速度３．５ｍ／秒で積極的に飛行する様子を示す（図５（ｂ））。水平位置は多少のドリフトを伴ったグランドツルースに類似する（図５（ａ））。 搭載カメラからの画像（図６（ａ）〜図６（ｄ））と、外部カメラからの画像（図６（ｅ）〜図６（ｈ））である。オープンスペース（空間）、立木、複雑な建物構造、および屋内環境を含んだ多様な環境が存在する。ＭＡＶの位置は円で示されている。実験のビデオは添付ビデオおよびhttp://mrsl.grasp.upenn.edu/shaojie/ICRA2014.mp4で観ることができる。 搭載カメラからの画像（図６（ａ）〜図６（ｄ））と、外部カメラからの画像（図６（ｅ）〜図６（ｈ））である。オープンスペース（空間）、立木、複雑な建物構造、および屋内環境を含んだ多様な環境が存在する。ＭＡＶの位置は円で示されている。実験のビデオは添付ビデオおよびhttp://mrsl.grasp.upenn.edu/shaojie/ICRA2014.mp4で観ることができる。 搭載カメラからの画像（図６（ａ）〜図６（ｄ））と、外部カメラからの画像（図６（ｅ）〜図６（ｈ））である。オープンスペース（空間）、立木、複雑な建物構造、および屋内環境を含んだ多様な環境が存在する。ＭＡＶの位置は円で示されている。実験のビデオは添付ビデオおよびhttp://mrsl.grasp.upenn.edu/shaojie/ICRA2014.mp4で観ることができる。 搭載カメラからの画像（図６（ａ）〜図６（ｄ））と、外部カメラからの画像（図６（ｅ）〜図６（ｈ））である。オープンスペース（空間）、立木、複雑な建物構造、および屋内環境を含んだ多様な環境が存在する。ＭＡＶの位置は円で示されている。実験のビデオは添付ビデオおよびhttp://mrsl.grasp.upenn.edu/shaojie/ICRA2014.mp4で観ることができる。 搭載カメラからの画像（図６（ａ）〜図６（ｄ））と、外部カメラからの画像（図６（ｅ）〜図６（ｈ））である。オープンスペース（空間）、立木、複雑な建物構造、および屋内環境を含んだ多様な環境が存在する。ＭＡＶの位置は円で示されている。実験のビデオは添付ビデオおよびhttp://mrsl.grasp.upenn.edu/shaojie/ICRA2014.mp4で観ることができる。 搭載カメラからの画像（図６（ａ）〜図６（ｄ））と、外部カメラからの画像（図６（ｅ）〜図６（ｈ））である。オープンスペース（空間）、立木、複雑な建物構造、および屋内環境を含んだ多様な環境が存在する。ＭＡＶの位置は円で示されている。実験のビデオは添付ビデオおよびhttp://mrsl.grasp.upenn.edu/shaojie/ICRA2014.mp4で観ることができる。 搭載カメラからの画像（図６（ａ）〜図６（ｄ））と、外部カメラからの画像（図６（ｅ）〜図６（ｈ））である。オープンスペース（空間）、立木、複雑な建物構造、および屋内環境を含んだ多様な環境が存在する。ＭＡＶの位置は円で示されている。実験のビデオは添付ビデオおよびhttp://mrsl.grasp.upenn.edu/shaojie/ICRA2014.mp4で観ることができる。 搭載カメラからの画像（図６（ａ）〜図６（ｄ））と、外部カメラからの画像（図６（ｅ）〜図６（ｈ））である。オープンスペース（空間）、立木、複雑な建物構造、および屋内環境を含んだ多様な環境が存在する。ＭＡＶの位置は円で示されている。実験のビデオは添付ビデオおよびhttp://mrsl.grasp.upenn.edu/shaojie/ICRA2014.mp4で観ることができる。 衛星画像と整合された飛行体の飛行経路を図示する。異なる彩色はセンサモードの異なる組み合わせを示す。Ｇ＝ＧＰＳ、Ｖ＝ビジョン、Ｌ＝レーザ 経時的なセンサの利用を図示する。全てのセンサに不具合が発生している。これは、この種の屋内−屋外の任務にはマルチセンサの融合が必須であることを示す。 飛行体が建物密集地域を通って飛行する際の共分散の変動を示す（２００ｓから３００ｓの間、図７の上部）。ＧＰＳは建物の陰によって有効状態になったり無効状態になったりした。共分散ｘ、ｙおよび偏飛行（一時的逸脱）はＧＰＳが無効化すると増加し、ＧＰＳが復活すると減少した。ボディフレームの速度はＧＰＳ測定値に関係なく観察が可能であり、よってその共分散は小さいままであった。速度の共分散のスパイク現象は飛行体が太陽に直面していることによる。Ｘ−Ｙ共分散は共分散の部分行列のフロベニウスノルムから計算される。 衛星マップ上に重ねられた飛行体の飛行経路を図示する。飛行体は並木が立ち並んだキャンパス環境で運用されており、運用中にＧＰＳが不調になる高リスクが介在した。 並木が立ち並んだキャンパス環境をＭＡＶが自律的に飛行する際の搭載（図１１（ａ））および外部（図１１（ｂ））カメラの画像を示す。少なからぬ外光の状態に着目。 並木が立ち並んだキャンパス環境をＭＡＶが自律的に飛行する際の搭載（図１１（ａ））および外部（図１１（ｂ））カメラの画像を示す。少なからぬ外光の状態に着目。 ここで説明する主題の１実施例によるマルチセンサ融合を実行する回転翼型飛行体ＭＡＶのブロック図である。 ここで説明する主題の１実施例による回転翼型飛行体ＭＡＶの自律性を制御するマルチセンサ融合の例示的プロセスを図示するフローチャートである。 限定された搭載コンピュータ（インテルＡｔｏｍ１．６ＧＨｚプロセッサ）およびセンサ（魚眼レンズと既製の安価なＩＭＵを備えた２つのカメラ）を備えた実験プラットフォームを図示する。プラットフォーム重量は７４０ｇであった。 マークされたモジュール間の更新レートと情報フローを備えたシステム構築を図示する。 セクションＶＩＩＩ−Ａで提示されている飛行経路の自律トラッキング（追跡）中のボディフレーム速度の推定の成果を示す。 急速平行移動（図１７（ａ）〜図１７（ｂ））および急速回転移動（図１７（ｃ）〜図１７（ｄ））による特性トラッキング性能に対する影響を図示する。トラッキングされた特徴部の数は回転移動後に大きく減少した。 急速平行移動（図１７（ａ）〜図１７（ｂ））および急速回転移動（図１７（ｃ）〜図１７（ｄ））による特性トラッキング性能に対する影響を図示する。トラッキングされた特徴部の数は回転移動後に大きく減少した。 急速平行移動（図１７（ａ）〜図１７（ｂ））および急速回転移動（図１７（ｃ）〜図１７（ｄ））による特性トラッキング性能に対する影響を図示する。トラッキングされた特徴部の数は回転移動後に大きく減少した。 急速平行移動（図１７（ａ）〜図１７（ｂ））および急速回転移動（図１７（ｃ）〜図１７（ｄ））による特性トラッキング性能に対する影響を図示する。トラッキングされた特徴部の数は回転移動後に大きく減少した。 シミュレーションされたクアドロータ（四回転翼体）が一連の中間地点から発生されたスムーズな飛行経路をトラッキングしていることを示す。飛行経路の復帰は２０秒（ｓ）での中間地点の変化後に発生した。 シミュレーションされたクアドロータ（四回転翼体）が一連の中間地点から発生されたスムーズな飛行経路をトラッキングしていることを示す。飛行経路の復帰は２０秒（ｓ）での中間地点の変化後に発生した。 実験時にオペレータに飛行体（運搬体）と交信させることができるＭＡＶのナビゲーション（航行）に対する有限ステートマシンベースアプローチを示す。 ロボット（自動機）が長方形パターンから発生されたスムーズな飛行経路に従うようにコマンドされたときの所望飛行経路、推定飛行経路および実際の飛行経路を示す。 ロボット（自動機）が長方形パターンから発生されたスムーズな飛行経路に従うようにコマンドされたときの所望飛行経路、推定飛行経路および実際の飛行経路を示す。 搭載カメラにより撮影された画像（図２１（ｂ））と共に、屋内環境のスナップ写真画像（図２１（ａ））である。床は無特徴であり、下向カメラに依存するアプローチにとっては挑戦的であり得る。 搭載カメラにより撮影された画像（図２１（ｂ））と共に、屋内環境のスナップ写真画像（図２１（ａ））である。床は無特徴であり、下向カメラに依存するアプローチにとっては挑戦的であり得る。 屋内ナビゲーション実験時のマップと推定位置を示す。ループの閉鎖後にＳＬＡＭを介して得られた位置推定の無視できない不連続性に着目（図２２（ｃ））。 屋内ナビゲーション実験時のマップと推定位置を示す。ループの閉鎖後にＳＬＡＭを介して得られた位置推定の無視できない不連続性に着目（図２２（ｃ））。 屋内ナビゲーション実験時のマップと推定位置を示す。ループの閉鎖後にＳＬＡＭを介して得られた位置推定の無視できない不連続性に着目（図２２（ｃ））。 ループの閉鎖後の屋外実験の最終３Ｄマップと飛行経路を示す。 複雑な屋外環境における自律ナビゲーションの画像を含む。外部ビデオカメラと搭載カメラの両方からの画像が示されている。実験のビデオはhttp://mrsl.grasp.upenn.edu/shaojie/IROS2013.movで観られる。 複雑な屋外環境における自律ナビゲーションの画像を含む。外部ビデオカメラと搭載カメラの両方からの画像が示されている。実験のビデオはhttp://mrsl.grasp.upenn.edu/shaojie/IROS2013.movで観られる。 複雑な屋外環境における自律ナビゲーションの画像を含む。外部ビデオカメラと搭載カメラの両方からの画像が示されている。実験のビデオはhttp://mrsl.grasp.upenn.edu/shaojie/IROS2013.movで観られる。 複雑な屋外環境における自律ナビゲーションの画像を含む。外部ビデオカメラと搭載カメラの両方からの画像が示されている。実験のビデオはhttp://mrsl.grasp.upenn.edu/shaojie/IROS2013.movで観られる。

回転翼型小型飛行体（ＭＡＶ）は、その小型サイズ、卓越した動作能力、およびホバリング性能のため、閉鎖された屋内および屋外における調査、探索および救助のための理想的なプラットフォーム（ハードウェア環境）である。そのような任務では、オペレータ（操作員）の作業量を最少化するためにＭＡＶが自律飛行できることは非常に重要である。特にＭＡＶの本来的に敏捷な動態能力のため、安定したステート推定は必須である。費用とペイロード（荷重）のため、ほとんどのＭＡＶには、長期のステート推定が不能である安価な固有（自己）受容性センサ（例：ＭＥＭＳ ＩＭＵ）が装備されている。よって、通常はＧＰＳ、カメラおよびレーザスキャナのごとき外部受容性センサが固有受容性センサと融合されて推定精度を改善する。高度に開発されているＧＰＳベースのナビゲーション技術［文献１、２］以外に、レーザスキャナ［文献３、４］、一眼カメラ［文献５、６］、ステレオ（立体）カメラ［文献７、８］およびＲＧＢ−Ｄセンサ［文献９］を使用したＧＰＳが正常機能しない環境における自律飛行のための安定ステート推定に関する最近の文献が存在する。しかし、これら全てのアプローチは一定の環境条件下で機能を発揮するだけである１体の外部受容センサモードに依存している。例えば、レーザベースのアプローチは構造物環境を必要とし、ビジョン（視覚）ベースのアプローチは十分な明度と特徴部とを必要とするが、ＧＰＳは屋外で機能するだけである。従って、それらは、環境が大きく変動する可能性がある屋内−屋外移行が関与する大スケールの環境では機能を発揮しない可能性がある。そのような状況下では、ＧＰＳ、カメラ、およびレーザからの複数の測定値が利用でき、それら測定値の融合は向上した推定精度および安定性を提供する。
しかし、実際には、この余分な情報は無視されるか、センサの統合ソフト間の切換えに使用される［文献１０］。

この発明の主たる目的は、異なっており、且つ変動する時間間隔で絶対的または相対的な観察結果を提供する複数の異種センサからのノイズが混入した測定値を統合し、自律飛行のためにスムーズでグローバリに一貫した位置推定をリアルタイムで提供するモジュール形態で拡張が可能なアプローチを開発することである。この発明に中心的な存在である第１の主要な貢献は、フィルタリングフレームワークを使用して一貫した推定値が得られて維持される対象である増大されたステートベクトルを創出すべく、以前のステートの複製で飛行体のステートを増大することによって、文献［１１］に構築される相対測定結果を融合することへの原則的アプローチである。第２の重要な貢献は我々のアンセンテッドカルマンフィルタ（ＵＫＦ）である。そこでは、プロパゲーションステップと更新ステップによって、増大されたステートのための共分散マトリックス（行列）の半確定の結果である困難性が回避される。最後に、我々は、２つの建物に出入りする際の風速が約１０ｍｐｓの条件下で、平均速度１．５ｍ／秒での４４０メートルのジグザグ経路が関与する大スケールの屋内−屋外自律飛行ナビゲーション実験において、我々のフレームワークの安定性を示すため、新たな実験プラットフォーム（図１）によって得られた結果を示す。

次に、本発明の基礎である従来の技術を説明する。セクションＩＶのＵＫＦベースのセンサ融合スキーム（体系）の主要な貢献を提示する前に、セクションＩＩＩにおいてモデルフレームワークが概説されている。実験プラットフォームおよびセクションＶＩの実験結果の我々の説明において、我々は全ての着想を統合している。

ＩＩ．従来技術
我々は、全ての利用可能なセンサ情報を融合するために、カルマンフィルタの非線形（非一次）変量のごときコンスタントなコンピュータ計算の複雑性のフィルタ処理ベースのアプローチを適用することに関心がある。我々は、ＳＬＡＭベースのマルチセンサ融合アプローチ［文献１２、１３］は最良の結果を提供するが、自律制御を目的としたリアルタイムのステートフィードバックのためのコンピュータ処理はコスト高になることを強調する。

フィルタリング帰納（再帰）式にてＧＰＳ、圧力／レーザ高度計のごとき複数の絶対測定値を融合することが直接的であるが、レーザまたはビジュアルオドメータ（飛行距離計）から得られる複数の相対測定値の融合がそれ以上に関与する。相対測定値を蓄積するのは普通であり、以前のステート推定値はそれらを擬似絶対測定値として融合する［文献５、１４］。しかし、そのような融合は最良ではない。なぜなら、得られるグローバル位置および偏飛行共分散は実際の推定誤差と較べて矛盾しない程度に小さいからである。これは識別性プロパティに反しており［文献６］、そのことは、そのようなグローバル数量が実際的には識別不能であることを暗示する。よって我々は、マルチセンサからの複数の相対測定値を適用するときのステート不確実性を適切に説明するためにステートオーグメンテーション技術［文献１１］に基づいて我々の方法を開発した。

我々は、最低限のコード処理および数学的推論でセンサの簡単な追加と除去を可能にするモジュール形態のフレームワークの開発を目指した。我々は、一般的なＥＫＦベースの定式化［文献５、８］において、ヤコビアン（ヤコビ行列式）のコンピュータ計算がＭＡＶのごとき複雑なシステムにとっては問題化することに着目した。よって、我々は緩くカップリング（結合）している、推定が介在しないアンセンテッドカルマンフィルタ（ＵＫＦ）フレームワーク［文献１］を採用した。ＥＫＦからＵＫＦへの変更は幾つかの困難を提起する。それらはセクションＩＶ−Ａにおいて詳細に説明および対処されている。文献［１５］は本発明に類似している。しかしながら、文献［１５］のＥＫＦベースの推定量
は複数の相対測定値の融合をサポートしない。

ＩＩＩ．マルチセンサシステムモデル
我々はワールドフレームとボディフレームのベクトルをそれぞれ（・）^ωおよび（・）^ｂと定義する。簡単にするため、我々は全ての搭載センサがキャリブレーション処理を経ており、ボディフレームに取り付けられているものと想定する。ＭＡＣの主ステートは以下のように定義される：
ここで、ｐ^ｗ＝［ｘ^ｗ，ｙ^ｗ，ｚ^ｗ］^Ｔはワールドフレームの３Ｄポジション（位置）であり、Φ^ｗ＝［Ψ^ｗ，Θ^ｗ，φ^ｗ］^Ｔは偏飛行、上下振動および横揺れオイラー角であり、ワールドフレームのボディの３Ｄ指向性を表わしており、ボディフレームからワールドフレームへのベクトルの回転を表わす行列Ｒｗｂがそこから得られる。ｐ^ｂはボディフレームの３Ｄ速度である。
と
は加速度計とジャイロスコープのバイアスであり、それら両方はボディフレーム内で表現される。
はワールドフレームのレーザ高度計及び／又は圧力高度計のバイアスをモデル化している。

我々はＩＭＵベースのステートプロパゲーションモデルを考察する：
ここでｕはボディフレームの線状（一次）加速とＩＭＵからの角速度の測定値である。
はプロセスノイズである。ｖ_ａとｖ_ωはジャイロスコープおよび加速度計に関与する付加ノイズを表わす。ｖ_ｂａ、ｖ_ｂω、ｖ_ｂｚは、ジャイロスコープ、加速度計および高度計のバイアスのガウスランダムウォークをモデル化するものである。関数ｆ（・）は連続的時間動力学式のディスクリートバージョンである［文献６］。

ステートプロパゲーションにおけるエラー（誤差）を訂正（補正）するには通常では外部受容センサが使用される。文献［１１］に従い、我々は関与するセンサの特性に応じて
測定値を絶対的または相対的であると考える。我々は随意の数の絶対測定モデルまたは相対測定モデルを許容する。

Ａ．絶対測定値
全ての絶対測定値は次の形態でモデル化できる：
ここで、
は、加法性あるいは非加法性の測定ノイズである。ｈ_ａ（・）は一般的に非線形（非一次）関数である。絶対測定値は現在ステートをセンサ出力に接続する。その例はセクションＶ−Ｂで示されている。

Ｂ．相対測定値
相対測定値は現在ステートおよび過去ステートをセンサ出力と結び付ける。それを以下のよう表すことができる：
この式は、オドメータがステートの２つの時点間のインクレメンタル（増分的）変化量を測定するので、オドメトリ（路程測定）アルゴリズムの特性を正確にモデル化したものである（セクションＶ−ＣおよびセクションＶ−Ｄ）。我々はまた、時間ドリフトを回避するために、大抵の従来レーザ／ビジュアルオドメトリアルゴリズムがキーフレームベースであることに着目した。よって、我々は、同じ過去ステートｘ_ｔに対応する複数の未来測定値（ｍ∈Ｍ，｜Ｍ｜＞１）を許容した。

ＩＶ．ＵＫＦ−ベースのマルチセンサ融合
我々は、経験が浅いユーザであっても容易に拡張できるモジュール型センサ融合フィルタをデザインすることを望んだ。すなわち、センサの追加／削減のためのコード化および数学的偏差の量を最小にすることである。一般的なＥＫＦベースのフィルタ処理フレームワークの弱点は、ヤコビ行列を計算する必要性であるが、これは困難であり、複雑なＭＡＶシステムのために時間を要する作業であることが判明している。よって、我々は導関数が関与しないＵＫＦベースのアプローチ［文献１］を採用した。ＵＫＦの重要要素は、シグマポイントのプロパゲーションを介した非一次関数を利用したガウスランダムベクトルのプロパゲーションの近似計算（概算）である。
とし、非一次関数を考察した：
さらにＸを以下とする：
ここで、ｇ（・）は非一次関数であり、λはＵＫＦパラメータである。
は共分散行列平方根のｉ番目の縦列であり、通常はコレスキー分解を介して計算される。Ｘはシグマポイントと呼ばれるものである。ランダムベクトルｙの平均分散と、ｘとｙの間の交差共分散は次のように近似計算できる：
ここで、
と
はシグマポイントの加重値である。このアンセンテッド変換はステートプロパゲーションと測定値更新の両方における共分散のトラッキング維持に使用でき、よってヤコビアンベースの共分散近似計算の必要性が回避される。

Ａ．複数相対測定値のためのステートオーグメンテーション（増大）
相対測定値は現在ステートと過去ステートの両方に依存するので、測定は現在ステートだけに依存すべきであるというカルマンフィルタの根本的な想定に反する。この問題に対処する１つの方法はステートオーグメンテーション［文献１１］を介することである。そこでは過去ステートの複製がフィルタに維持される。ここで我々は、随意数の相対測定値モデルを取り扱うために、複数の測定値が同じ増大されたステートに対応するという見込みで、文献［１１］の拡張を提案する。我々の一般的なフィルタリングフレームワークは、絶対測定値モデルおよび相対測定値モデルの便利なセットアップ、追加および除去を許容する。

測定値はステートｘの全てのコンポーネント（構成要素）に影響を及ぼすことはない。例えば、ビジュアルなオドメトリは６−ＤＯＦ（６自由度）ポーズに影響するだけであり、速度やバイアスの観点には影響しない。我々はｉ番目の増大（補強：augment）された
ステートを
と定義する。
ｘ_ｉはｘの随意サブセットである。我々はバイナリ選択行列（binary selection matrix
）サイズｎ_ｉｘｎのＢ_ｉをｘ_ｉ＝Ｂ_ｉｘと定義する。ある時点を考察すると、Ｉ個の増大されたステートがフィルタ内に共分散と共に存在する：

新たな増大されたステートｘ_Ｉ＋１の追加は以下で実行できる：

同様に、増大されたステートｘ_ｊの除去は次のように実行される：
ここでは、
および
である。更新されている増大されたステート共分散は次のようになる：

オドメトリに類似する測定値モデルのキーフレームの変化は単に増大されたステートｘ_ｉの除去であり、同じＢ_ｉでの別な増大されたステートの追加が続く。我々は同じ増大されたステートに対応する複数の相対測定値を許容しているので、文献［１１］とは対照的に、測定値更新後には増大されたステートは削除されない（セクションＩＶ−Ｄ）。

このステートオーグメンテーションの系統的論述はＥＫＦセッティングでは有効であるが、我々がそれをＵＫＦに適用しようと試みると問題が発生する。これは、新しい増大されたステートの追加（式８）が本質的には主ステートの複製だからである。
得られる共分散行列
は正定値とはならなく、ステートプロパゲーションのためのコレスキー分解（式５）は有効に機能しないであろう（非唯一性）。我々は、ＥＫＦのためのヤコビ行列に類似してはいるが、陽関数表示的にヤコビアンを計算しない何かを有することを希望した。

Ｂ．ＵＫＦのためのヤコビアン
文献［１６］では、著作者は線形（一次）回帰カルマンフィルタ（ＬＲＫＦ）としてＵＫＦの新規な解釈を提示する。ＬＲＫＦにおいて我々は、分布の加重された離散（またはシグマポイント（式６））表現
が与えられたときの非一次関数（式４）の最良一次近似式ｙ＝Ａｘ＋ｂ＋ｅを求めた。その目的は回帰行列Ａと、線形化誤差ｅを最小化するベクトルｂを発見することであった：

文献［１６］で示されているように最良一次回帰は次の式で提供される：
式（９）の一次回帰行列Ａは非一次関数（式４）の一次近似として機能する。それはＥＫＦ式におけるヤコビアンに類似する。従って、ＵＫＦでのプロパゲーションおよび更新ステップはＥＫＦのごとき類似した手法で実行できる。

Ｃ．ステートプロパゲーション
ステートプロパゲーション時に主ステートだけが変化するという事実を観察して、我々
は増大されたステートと共分散（式７）を以下のように隔絶することから始めた：

増大されたステート（式７）に適用される非一次ステートプロパゲーション（式１）の一次近似は次のようになる：

そこから我々は全面的な増大されたステートのプロパゲーション（伝播）が実際には不要であることを理解することができた。なぜなら唯一の無視できない回帰行列が主ステートに対応するからである。我々は、
から発生されたシグマポイントを介して主ステートｘのみをプロパゲーションでき、交差共分散
を更新するためにＵＫＦヤコビアンＦ_ｔを使用することができた。主ステート
の共分散行列は常に正定値であるため、我々はコレスキー分解の不首尾の問題を回避した。

プロセスのノイズは付加的なものではないので、我々はプロセスノイズで主ステートを増大し、シグマポイントを次の式から発生させた：

次にそのステートは式（１１）を式（１）、（５）および（６）に代入してプロパゲーションされた。我々は、ｔまでの測定値が与えられると時間ｔ＋１でｘの推定値である
並びに
と
を取得した。式（９）に続いて次が知れた：

プロパゲーションされている増大されたステートおよびその共分散は式（１０）に従ってアップデートされた：

Ｄ．測定値更新
２つの測定値間にｍ個のステートプロパゲーションを存在させ、我々は最新測定値が到来したときに
と
を維持した。Ｊ番目の増大されたステートに依存する相対測定値（式３）を考えると、測定推定値およびその一次回帰近似値は以下であり得る：

同様に、主ステートと１つの増大されたステートのみが各測定値のアップデートに関与するので、我々は別な増大されたステートを、可能な非追加測定ノイズと共に構築できる：

ステートプロパゲーション（式１２）後、
は正定値として保証され、よって式（５）と式（６）でのごとくにシグマポイントプロパゲーションを実行することは安全である。我々は
および次の式を取得した：
我々はＥＫＦに類似する測定値のアップデートを適用した：
ｚ_ｔ＋ｍは実際のセンサ測定値である。主ステートおよび増大されたステートは測定値アップデート時に訂正されるであろう。我々は、不活性な増大されたステートに対応するＨ_{ｔ＋ｍ｜ｔ}のエントリがゼロであることに着目した。これは行列乗算を加速するのに利用が可能である。

絶対測定値の融合は、
および対応する絶対測定値モデル（式２）を適用することにより簡単に実行できる。

図９で示すように、複数の相対測定値の融合は緩やかな成長をもたらすが、グローバル位置および偏飛行における無限界共分散となる。これは、これらのグローバルな量が観察不能となる文献［６］の結果と一致する。

Ｅ．遅延した順序不順測定値のアップデート
複数の測定値を融合するとき、測定値がフィルタに順番が狂って到着することがある。すなわち、初期ステートに対応する測定値が後続ステートに対応する測定値の後に到着することがある。これはカルマンフィルタのマルコフ想定に反する。また、センサの処理遅延によって、測定値がステートプロパゲーションより遅れることもある。

我々は、測定値をプライオリティ列（優先順位付き待ち行列）に保存することで、これらの２つの問題に対処した。そこでは、プライオリティ列の上段は最古測定値に対応する。予め定義されている最大許容可能センサ遅延ｔ_ｄ１００ｍｓが我々のＭＡＶプラットフォームのために設定された。現在のステートからのｔ_ｄよりも古いステートに対応する新着の測定値（ステートプロパゲーションにより発生）は直接的に破棄された。各ステートプロパゲーション後に、我々はプライオリティ列をチェックし、ｔ_ｄよりも古いプライオリティ列内の全ての測定値を処理した。プライオリティ列は本質的に、現在のステートからのｔ_ｄより古くはない全ての測定値の測定値再順序化メカニズムとして機能する（図２）。フィルタ内で、我々は、そのステートを前方にプロパゲーションさせるため、常に最新のＩＭＵ測定値を利用した。しかしながら、我々は、需要に応じて共分散をプロパゲーションするだけであった。図２で示すように、共分散は、最後の測定時間から現在測定にプロパゲーションされるだけである。

Ｆ．グローバルポーズ測定値を扱う別方法
飛行体が環境内を移動する際にＧＰＳおよび磁力計からグローバルポーズ測定値が入手できる。グローバルポーズ測定値としてＧＰＳを融合し、最良のステート推定値を発生させるのは容易である。しかし、現実的な適用にとってそれは最善ではないかもしれない。ＧＰＳが利用できない環境内で運用される飛行体は蓄積されたドリフトの影響を受けるかもしれない。飛行体がＧＰＳ信号を図３（ａ）で示すように取得するとき、ＧＰＳ測定値と推定ステート（ｚ_５〜ｓ_５）との間には大きな不一致が存在するかもしれない。ＧＰＳをグローバル測定値として直接的に適用することは、推定（大きな線形化誤差）と制御（急激ポーズ変化）の両方に不都合な現象を招く結果となるであろう。

これは新規な問題ではなく、それはローカルフレームベースのナビゲーションの観点で地上の運搬体のために研究されてきた［文献１７］。しかし文献［１７］は運搬体の合理的に正確なローカル推定値は常に利用可能であると想定する（例：ホイールオドメトリ）。これはＭＡＶには当てはまらない。なぜなら、搭載ＩＭＵだけのステート推定値は常に数秒内で大きくドリフト離脱するからである。ＩＭＵドリフトとホイールオドメトリとの主たる相違点は、ＩＭＵは時間的にドリフトするが、ホイールオドメトリはスペース的にのみドリフトすることである。しかし我々は、時間的にドリフト無介在である推定値を発生させることができる相対的外部受容センサを有した。よって我々は、全ての相対的外部受容センサが機能停止した場合のみに対処する必要があるだけであった。従って我々のゴールは、相対的センサ不調時（同士）間のギャップを埋めるためにグローバルＧＰＳ測定値をローカルフレームに適切に変換することである。
が２ＤポーズであるポーズのみのグラフＳＬＡＭを考察する。我々は、レーザ／ビジュアルオドメトリからのインクレメンタルモーション制限ｄ_ｋ、スペースループクロージャ（空間ループ閉鎖）制限１_ｋおよびＧＰＳからの絶対ポーズ制限ｚ_ｋが与えられとき、ポーズグラフの最良の形態（相対配置）を発見することを試みた：

最良のポーズグラフ形態は図３（ｂ）で示すような利用可能な解決策によって発見できた［文献１８］。このポーズグラフは２つのノード間に相対的外部受容測定値が存在しなければ断続される。２つのポーズグラフをｋ−ｌとｋとの間で断絶させてみた。
ポーズグラフＳＬＡＭは非最良ｓ_ｋ−ｌとＳＬＡＭ最良
ステートとの間の変換を提供する。この変換は、グローバルＧＰＳ測定値をｓ_ｋ−１と整合するように変換させるのに利用できる：
ここで、
と
は文献［１９］において定義されているようなポーズ複合オペレーションである。△_ｔ−１の共分散
と、続く
の共分散
は文献［１９］に従って計算できる。この式は
と
との間の不一致を最小化し、ステート推定におけるスムーズ性（滑らかさ、平滑度）を維持する。変換された
は絶対測定値としてＵＫＦ（Ｆｉｇ．４（ａ））に対して適用される。

しかし我々のフィールド実験（セクションＶＩ）における大スケールにもかかわらず、我々は、蓄積されたドリフトが直接的ＧＰＳ融合の問題を引き起こすのに十分に大きいという場合をほとんど発見しなかった。将来的には、我々は、上述のローカルフレームベースのアプローチの必要性を確認するために、さらに大きなスケールの実験を実施するであろう。

Ｖ．実施の詳細
Ａ．実験プラットフォーム
図１で示す実験プラットフォームは、アセンディングテクノロジーＧｍｂＨ（社）（http://www.asctec.de/）のペリカンクアドロータに基づいている。このプラットフォーム
はＩＭＵとユーザプログラマブルＡＲＭ７マイクロコントローラで成るオートパイロトボードを本来的に備えている。搭載された主計算装置は、１．８ＧＨｚコアｉ３プロセッサを備えたインテルＮＵＣであり、８ＧＢのＲＡＭと１２０ＧＢのＳＳＤを備えていた。センサ統合ソフトはｕｂｌｏｘ ＬＥＡ−６Ｔ ＧＰＳモジュール、ホクヨＵＴＭ−３０ＬＸ ＬｉＤＡＲおよび２５Ｈｚで７５２ｘ４８０の画像を撮影する魚眼レンズを備えた２体のｍｖＢｌｕｅＦＯＸ−１ＶＩＬＣ２００ｗグレイスケールＨＤＲラメラを含んだ。我々はフレーム同期化のためのハードウェアトリガ動作を活用した。この搭載自動露出コントローラは微調整によって急速明度変化時に急速適応することができる。３Ｄプリントされたレーザハウジングは高度測定のためのレーザビームの一部を方向変化させる。プラットフォームの全重量は１．８７ｋｇであった。全アルゴリズムはインターフェースロボットミドルウェアとしてのロボット操作システム（ＲＯＳ）（http://ros.org）を使用してＣ_＋＋にて開発された。

Ｂ．絶対測定値
搭載センサによっては絶対測定値を発生させることができる（セクション１１１−Ａ）。以下はその詳細である：

１）ＧＰＳおよび磁力計：
２）レーザ／圧力高度計：
３）擬似重力ベクトル：もしＭＡＶがホバリング（空中浮遊）状態あるいはほぼ一定の速度で移動しているなら、我々は、加速度計の出力が重力ベクトルの擬似測定値を提供すると述べるであろう。ｇ＝［０，０，ｇ］^Ｔとすると、以下が得られた：

Ｃ．相対測定値−レーザベースオドメトリ
我々は、我々が過去に開発したレーザベースのオドメトリ［文献４］を活用した。人工屋内環境はたいていの場合に垂直壁を含むという観察に基づき、我々は２．５Ｄの環境を想定した。この想定に基づき、我々はレーザスキャナを共通の地表に投射するために搭載された横揺れと縦揺れの推定値を利用することができた。このように、飛行体のインクレメンタル（増分的）水平動作の推定に２Ｄスキャンマッチングが利用できる。我々はホバリング最中のドリフトを回避するためにローカルマップを保持した。
ここで、
であり、
および
は文献［１９］で定義されているような２Ｄポーズ複合オペレーション（運用）である。

Ｄ．相対測定−ビジュアルオドメトリ
我々は、旧来のキーフレームベースビジュアルオドメトリアルゴリズムを実施した。キ
ーフレームベースのアプローチは時間ドリフト現象が発生しない利点を有する。我々は軽量コーナ特徴部を利用するが、高レート（２５Ｈｚ）でアルゴリズムを実行するように選択した。特徴部はＫＬＴトラッキング装置を介して複数の画像でトラッキングされた。三角測量された特徴ポイントのセットを備えたキーフレームが与えられると、我々は、キーフレームに関して飛行体の６−ＤＯＦモーションを推定するために、安定した反復２Ｄ−３Ｄポーズ推定を実行した［文献８］。移動した距離および有効な３Ｄポイントの現在数に応じて新キーフレームが挿入された。

Ｅ．フィードバック制御
大きな方向性（配向性）の変更を伴うこともある異なる環境に跨る安定した飛行を達成するため、我々は非線形誤差計量を伴う位置トラッキングコントローラ［文献２０］を使用することを選択した。１００Ｈｚのフィルタ出力（セクションＩＶ）がコントローラのためのフィードバックとして直接的に使用された。我々の実践では、飛行姿勢コントローラはＭＡＶの自動操縦ボード上のＡＲＭプロセッサで１ｋＨｚにて運用され、位置トラッキングコントローラは主コンピュータで１００Ｈｚにて運用された。我々は、フレキシブルな運用を許容するために、セットポイント飛行経路トラッキングおよび速度コントロールの両方を実施した。

ＶＩ．実験結果
我々のシステムの安定性を示すために幾つかの実験が実施された。我々は、モーションキャプチャ（捕獲）システムを備えた実験室環境内での量的評価から始めた。続いて我々は我々のシステムを、産業複合団地および並木道キャンパスを含んだ２つの現実世界自律飛行実験で試験した。

Ａ．推定値性能の評価
我々は我々の搭載推定装置の限界にチャレンジすることを希望した。従って、我々はパイロットにクアドロータを積極的に最大３．５ｍ／ｓの速度にて４０°までの大きな飛行姿勢で飛行させた。搭載ステート推定値は、モーションキャプチャシステムからのグランドツルースに匹敵した。屋内でのＧＰＳ測定値は存在しないため、我々のシステムはレーザおよびビジョンからの相対測定値の融合に依存した。我々は２．５Ｄの想定（セクションＶ−Ｃ）に反する大きな飛行姿勢のために時にレーザ不調を観察した。しかし、マルチセンサフィルタはそれでも飛行体のステートを全体的にトラッキングした（図５）。非制限状態であったので、我々は絶対ポーズ誤差を定量化しなかった。しかしボディフレーム速度（図５（ｂ））はそれぞれｘ、ｙ、ｚ｛０．１０２１，０．１１８５，０．０７５５｝^Ｔ（ｍ／ｓ）の標準偏差のグランドツルースにほぼ匹敵した。

Ｂ．大スケール屋内および屋外環境での自律飛行
我々は厳しい条件の産業コンプレックス（複合団地）で我々のシステムを試験した。試験サイトは多様な環境を含んでおり、屋内空間、密集林、入り乱れた高層建物領域および屋内環境が含まれていた（図６）。ＭＡＶは搭載ステート推定を使用することで自律的に制御された。しかし、人間のオペレータは常に高レベルの中間地点または高レベル速度コマンドを飛行体に送るという選択をした。トータル飛行時間は約８分であり、飛行体は４４５ｍを平均速度１．５ｍ／秒で飛行した。マップ整合経路（図７）で示すように、実験中には頻繁にセンサ不調が発生し（図８）、マルチセンサ融合の必要性を示した。図９は
飛行体がＧＰＳ陰領域を飛行したときの共分散の進化を示す。グローバルなｘ、ｙおよび偏飛行誤差はＧＰＳ測定によって制限されたが、それがなければ誤差は制限なく広がったであろう。これは観察可能性分析結果と合致した。ＧＰＳの利用性に拘わらずボディフレーム速度に関する誤差が広がらないことは着目すべきである。図９の速度共分散におけるスパイク現象は直接陽光に面したカメラによるものであった。

Ｃ．並木キャンパスでの自律飛行
我々は図１０に示すように並木キャンパス環境でも実験を行った。このような環境での自律飛行は困難である。なぜなら飛行体が立木の陰に出入りするとき無視できない明暗条件の変化があるからである。飛行体の上方の樹木によるＧＰＳ不調のリスクもまた非常に大きい。レーザベースオドメトリは建物に接近しているときのみ機能する。全経路長は２８１ｍであった。

ＶＩＩ．結論および将来の活動内容
ここでの開示において、異なっており変動する時間間隔で絶対観測値または相対観測値を生み出す複数の異質センサからのノイズを含んだ測定値を統合するために、我々はモジュール形態の拡張可能なアプローチを提供する。我々のアプローチは自律飛行のための高レートのステート推定値をリアルタイムで発生させる。提案されているアプローチは複数の異質センサを装備した我々の新１．９ｋｇＭＡＶプラットフォームを機上で操作する。我々は、産業団地および並木キャンパスを通過する大スケールの屋内および屋外自律飛行実験での我々のフレームワークの安定性を実証した。

近い未来に大型複合施設環境を通過する完全な自律運行を達成するため、我々はＭＡＶプラットフォーム上でのさらに高いレベルの計画策定と状況認識の統合を図った。

図１２は、ここで説明する主題の１実施例に従った絶対測定値および相対測定値の両方を発生させるセンサからの測定値の融合のためのＭＡＶを図示するブロック図である。図１２では、ＭＡＶ１００は１以上のロータ１０４を使用してＭＡＶの動きを制御するための１以上のモータ１０２を含む。上述したように、ここで説明する実験では、アセンディングテクノロジー社から入手できるペリカンクアドロロータが使用された。しかし、他の回転翼飛行体であってもここで説明する主題のスコープからは逸脱せずに代用できる。それはまたセンサ入力に基づいてモータ１０２の操作を制御するためのコントローラ１０６を含む。コンピュータ装置１０８は、複数のセンサからの測定値を融合し、コントローラ１０６への出力信号を発生させるセンサ融合モジュール１１０を含む。図示の例では、センサ融合モジュール１１０は、ＩＭＵ１１２、圧力高度計１１４、磁力計１１６、レーザスキャナ１１８、ＧＰＳレシーバ１２０、カメラ１２２および圧力高度計１２３からの入力を受領する。センサ融合モジュール１１０は、レーザスキャナ１１８およびカメラ１２２によって発生された測定値等の相対測定値を上述のごとく増大されたステートに応じた測定値に変換する。変換された測定値は、前述したアンセンティドカルマンフィルタを使用して組み合わされ、コントローラ１０６に出力される。コントローラ１０６への出力として提供される信号は、ＭＡＶ１００の位置、速度、および加速度の制御のためのコントローラ１０６へのフィードバックとして活用される。コントローラ１０６はユーザ特定中間地点に到着するのに必要なＭＡＶ１００の飛行経路を推定する飛行経路エスティメータ１２４からの入力も受領する。

図１３は、ここで説明する主題の１実施例によるマルチセンサ融合を活用した回転翼飛行体ＭＡＶの動きを制御するための例示的プロセスを図示するフローチャートである。図１３のステップ２００では、複数の異なるモード（様式、形式）のセンサから入力が受領される。例えば、コンピュータ装置１０８とセンサ融合モジュール１１０は、そこから所与の時間に出力が入手できる図１２に図示した１以上のセンサから入力を受領できる。ス
テップ２０２で、以前のステートに依存するセンサの一部によって発生される相対出力測定値が、増大されたステートに依存する測定値に変換される。そのような変換を実行するプロセスはセクションＩＶ（Ａ）において前述されている。ステップ２０４において、異なるセンサが組み合わされてフィルタリングされた。例えば、測定値はアンセンティドカルマンフィルタを使用して組み合わせることができる。ステップ２０６において、組み合わされた測定値はユーザによって入力された中間地点と共に飛行経路ジェネレータ（飛行経路生成器）に出力される。ステップ２０８において飛行経路ジェネレータは回転翼飛行体ＭＡＶの動きを制御するために使用される。

以下の引用文献のそれぞれの開示内容は全て本願に援用されているものとする。

［１］Ｓ．Ｊ．ジュリエールおよびＪ．Ｋ．アールマン「非線形システムへのカルマンフィルタの新規拡張」ＳＰＩＥ（国際光学技術協会）会報、Ｉ．カダール著、第３０６８巻、１９９７年７月、１８２〜１９３ページ
［２］Ｒ．Ｖ．Ｄ．マーウィ、Ｅ．Ａ．ワンおよびＳ．Ｉ．ジュリエール「非線形推定のためのシグマポイントカルマンフィルタ：集積ナビゲーションへの適用」ＡＩＡＡ（米国航空宇宙工学協会）ガイダンス、ナビゲーションおよび制御会議、会報、プロビデンス市、ロードアイランド州、２００４年８月
［３］Ａ．バカラッチ、Ｓ．プレンティス、Ｒ．ヒーおよびＮ．ロイ「ＧＰＳ否定環境でのＲＡＮＧＥ安定自律ナビゲーション」実地ロボット工学誌、第２８巻５号、６４４６６６ページ、２０１１年
［４］Ｓ．シェン、Ｎ．ミッチェルおよびＶ．クマール「コンピュータ的に規制されたＭＡＶでの自律複床屋内ナビゲーション」ロボットおよび自律に関するＩＥＥＥ国際会議、会報、上海市、中国、２０１１年５月、２０〜２５ページ
［５］Ｓ．ワイズ、Ｍ．Ｗ．アキテリック、Ｓ．ライネン、Ｍ．チリおよびＲ．シーグワート「未知環境におけるリアルタイム搭載ビジュアル慣性ステート推定およびｍａｙｓの自動キャリブレーション」ロボットおよび自律に関するＩＥＥＥ国際会議、会報、セントポール市、ミネソタ州、２０１２年５月、９５７〜９６４ページ
［６］Ｄ．Ｇ．コッタス、Ｊ．Ａ．ヘッシュ、Ｓ．Ｌ．バウマンおよびＳ．Ｉ．ロウメリオティス「ビジョン補助慣性ナビゲーションの一貫性」エクスポロボット国際シンポジウム、会報、ケベック州、カナダ国、２０１２年６月
［７］Ｆ．フランドルファ、Ｌ．ヘング、Ｄ．ホネガー、Ｇ．Ｈ．リー、Ｌ．メイア、Ｐ．タンスカネンおよびＭ．ポレフェイズ「クアドロータＭＡＶを利用したビジョンベースの自律マッピングおよび開発」ｂｚｔｅｌｌ．ロボットおよびシステムに関するＩＥＥＥ／ＲＳＪ国際会議、会報、ビラモウラ、アルガルベ、ポルトガル国、２０１２年１０月
［８］Ｋ．シュミット、Ｔ．トーニオ、Ｅ．ルエス、Ｈ．ヒルクルニューラおよびＭ．スッパ「ロボットを飛行させるステレオビジョンベースの屋内／屋外ナビゲーション」知的ロボットおよびシステムに関するＩＥＥＥ／ＲＳＪ国際会議、会報、Ｃｏｚｚｉ、日本国東京、２０１３年１１月
［９］Ａ．Ｓ．ファン、Ａ．バクラッチ、Ｐ．ヘンリ、Ｍ．クライニン、Ｄ．マツラナ、Ｄ．フォックスおよびＮ．ロイ「ＲＧＢ−Ｄカメラを使用した自律飛行のためのビジュアルオドメトリおよびマッピング」ロボット研究国際ｓｐｉｔ．会報、フラッグスタッフ市、アリゾナ州、２０１１年８月
［１０］Ｔ．トミック、Ｋ．シュミット、Ｐ．ルッツ、Ａ．ドメル、Ｍ．カセッカ、Ｅ．メイア、Ｉ．Ｌ．グリクサ、Ｆ．ルエス、Ｍ．スッパおよびＤ．バーシュカ「自律ＵＡＶ：屋内および屋外都市部探索および救助のための研究プラットフォーム」ロボットおよび自律に関するＩＥＥＥ、会報、第１９巻３号、４６〜５６ページ、２０１２年
［１１］Ｓ．Ｉ．ロウメリオティスおよびＪ．Ｗ．バーディック「確率的なクローン技術：相対ステート測定値処理のための汎用フレームワーク」ロボットおよび自律に関するＩＥＥＥ国際会議、会報、ワシントンＤＣ、２００２年５月、１７８８〜１７９５ページ
［１２］Ｊ．カールソン「ＧＰＳ補助ＳＬＡＭでの大型都市環境マッピング」博士論文、ＣＭＵ、ピッツバーグ市、ペンシルバニア州、２０１０年７月
［１３］Ｄ．シュライチャ、Ｌ．Ｍ．バーガサ、Ｍ．オカア、Ｒ．バレアおよびＥ．ロペス「都市環境におけるリアルタイム階層ＧＰＳ補助ビジュアルＳＬＡＭ」ロボットおよび自律に関するＩＥＥＥ国際会議、会報、神戸市、日本国、２００９年５月、４３８１〜４３８６ページ
［１４］Ｓ．シェン、Ｙ．ムルガオンカ、Ｎ．ミッチェルおよびＶ．クマール「四回転翼での高速飛行のためのビジョンベースステート推定および飛行経路制御」ロボット：科学とシステム、会報、ベルリン市、ドイツ国、２０１３年
［１５］Ｓ．ライネン、Ｍ．Ｗ．アクテリック、Ｓ．ワイズ、Ｍ．チリおよびＲ．シーグワート「ＭＡＹナビゲーションに適用される安定モジュール形態マルチセンサ融合アプローチ」知的ロボットとシステムに関するＩＥＥＥ／ＲＳＪ国際会議、会報、東京都、日本国、２０１３年１１月
［１６］Ｔ．レフェブア、Ｈ．ブルイニンクスおよびＪ．Ｄ．シュラ「フィルタおよび推定装置における手段と共分散の非線形変換のための新規方法に対するコメント」変換、自律、制御に関するＩＥＥＥ、第４７巻８号、１４０６〜１４０９ページ、２００２年
［１７］Ｄ．Ｃ．ムーア、Ａ．Ｓ．ファン、Ｍ．ウォルタおよびＥ．オルソン「ロボットナビゲーションのための同時的ローカルおよび全地球的ステート推定」ロボットおよび自律に関するＩＥＥＥ国際会議、会報、神戸市、日本国、２００９年５月、３７９４〜３７９９ページ
［１８］Ｒ．クエマール、Ｇ．グリセッティ、Ｈ．ストラスダット、Ｋ．コンオルジェおよびＷ．バーガード「Ｇ２０：グラフ最良化のための汎用フレームワーク」ロボットおよび自律に関するＩＥＥＥ国際会議、会報、上海市、中国、２０１１年５月、３６０７〜３６１３ページ
［１９］Ｒ．スミス、Ｍ．セルフおよびＰ．チーズマン「ロボット不確定空間関係の推定」ロボットと自律に関するＩＥＥＥ国際会議、会報、第４巻、ラレイ市、ノースカロライナ州、１９８７年３月、８５０ページ
［２０］Ｔ．リー、Ｍ．レオキおよびＮ．マクラムロック「ＳＥ（３）上の四回転翼ＵＡＶの地形的追跡制御」決定および制御国際会議、報告書、アトランタ市ジョージア州、２０１０年１２月、５４２０〜５４２５ページ

上述したように、ここで説明する主題の１実施例による自律回転翼飛行体ＭＡＶは、飛行中の回転翼飛行体ＭＡＶの経路を、回転翼飛行体ＭＡＶの推定された現在ステートおよびユーザによる中間地点の入力に基づいて制御するための経路計画を発生させる飛行経路発生器またはエスティメータ（推定装置）１２４を含むことができる。以下の説明は、ここで説明する主題の１実施例に従った飛行経路発生器またはエスティメータ１２４によって実行できる経路計画を説明する。

クアドロータでの複合環境におけるビジョンベース自律ナビゲーション
ここで説明する主題は、前方対面カメラと安価なＩＭＵのみを装備した軽量クアドロータに複合環境を自律的にナビゲーションし、効率的にマッピングさせることができるシステムデザインを提供する。我々は、高レート搭載ビジョンベースのステート推定と制御、低レート搭載ビジュアルＳＬＡＭ、およびオンライン計画および飛行経路発生アプローチの安定した統合に焦点を合わせた。急激な中間地点変更および大スケールのループ閉鎖等の困難な条件下での滑らかな飛行経路の安定した追跡が達成された。提案されているシステムの性能は、複雑な屋内および屋外環境での実験を介して実証されている。

Ｉ．始めに
クアドロータ小型飛行体（ＭＡＶ）は、その小型サイズと卓越した俊敏性によって、閉じられた屋内環境および屋外環境における調査および探索並びに救助のための理想的なプ
ラットフォームである。このような目的においては、オペレータの作業量を最少化するにはクアドロータが自律的であることが必須である。このオペレーションにおいては、我々は、複雑な未知の屋内および屋外環境の自律的なナビゲーションのために、リアルタイムでロボットに搭載された必須の制御用コンピュータ装置を運用し、搭載センサのみを使用した軽量の既製クアドロータの利用に関心があった。

自律飛行ナビゲーションの問題は、この数年間、広く研究されてきた。初期の研究成果［文献１〜３］は主センサとして主にレーザスキャナに依存し、垂直方向にさほど大きくは形状変化しない構造要素を備えた屋内環境（２．５Ｄ想定）に飛行体を局所化している。ステート推定のための、相当量のペイロード（荷重）を加えるメカニカルパニングレーザスキャナが文献［４、５］で使用されている。文献［６〜８］でのようなビジョンベースのアプローチは屋内環境及び／又は屋外環境において安定した自律飛行を達成するために、下方対面カメラ、ステレオビジョンと下方対面光学フローセンサの組み合わせ、並びにＲＧＢ−Ｄセンサにそれぞれ依存する。しかし、これらアプローチは、環境構築想定、減少したアルゴリズム更新レートまたは大型飛行体サイズに起因する実際的な問題によって、クアドロータプラットフォームの機動性および操作性を利用することができない。加えて、下方対面ビジョンセンサに依存するアプローチ［文献６、７］は、無特徴床面または低高度の環境では安定して機能することができないことが多い。

スペクトルの他方端には、計量的なステート推定には依存しない多数の連続反応ナビゲーションアプローチ［文献９、１０］が存在する。これらアプローチは低計算能力で自律飛行を可能にするが、複雑環境での操作の際には基本的にＭＡＶの飛行能力を制限する。

我々は、飛行体に未知および未構築の環境における自身のステートを推定させ、その環境をマッピングさせ、そのマップでプラン（計画）させ、そのプランから展開される飛行経路に沿って自律制御させることができる自律ナビゲーションを追い求めた。オンラインによる障害物検出および再計画は、１ｍ／秒を超える平均飛行速度での静的環境および動的環境での運行を可能にする。そのような速度では、プラットフォームの敏捷性によって、短待時間ステート推定、オンラインスムーズ飛行経路発生および反応性飛行体制御が必要になる。この目標の達成において生じる困難は、同時的な局所化およびマッピングによるループ閉鎖中においてさえも、推定されたポーズがスムーズで一貫していることを保証する必要性である。伝統的に、ループ閉鎖補正値は高レートの搭載ステートエスティメータに直接的に融合される。これで、推定されたステートにおける断絶が発生するが、これは、特に急速起動中に、クアドロータの壊滅的な墜落を引き起こしかねない。

この研究において、我々は、主センサとして２体の前向カメラを採用するシステム構成を提案し、複雑な環境において効率的な自律ナビゲーションを可能にする再計画並びにループ閉鎖中の推定スムーズ性および制御安定性を維持する新規な方法論を提案することでこれらの必要性に対処する。

ＩＩ．システムデザインと方法論
我々はシステム構成および方法論、並びに我々のデザインに必要なハードウェアおよびソフトウェアのコンポーネントの大要を提供することから始める。主要コンポーネントの詳細な説明はシステムブロック図（図１５）の論理的流れ説明部に続くセクションで提供されている。

Ａ．ハードウェアプラットフォーム
実験プラットフォーム（図１４）はアセンディングテクノロジーズ（http:/www.asctec/de参照）のハミングバードクアドロータに基づいている。この既製プラットフォームは
、廉価なＩＭＵとユーザプログラム可能ＡＲＭ７マイクロコントローラを備えたオートパ
イロットボードを備えている。搭載されている高レベルコンピュータはインテルＡｔｏｍ１．６ＧＨｚのプロセッサおよび１ＧＢのＲＡＭを含んでいる。搭載コンピュータと地上ステーションとの間の交信は８０２．１１ｎの無線ネットワークを介して行われた。プラットフォームに新たに加える唯一のものはハードウェアＨＤＲを備えた２体のグレイスケールｍｖＢｌｕｅＦＯＸ−ＭＬＣ２００ｗカメラであった。全てのカメラには魚眼レンズが備わっている。カメラとＩＭＵとの間の同期化はハードウェアトリガリングを介して確実化された。プラットフォームの総重量は７４０ｇであった。

Ｂ．ソフトウェア構成および方法論
ソフトウェア構成は図１５に示されている。この構成は我々に搭載されている低レベル（低性能）および高レベル（高性能）のプロセッサ並びに機外地上ステーションとの間で計算を分割させた。搭載されている高レベルコンピュータにおいて、ビジョンベースエスティメータは２０Ｈｚで６−ＤＯＦポーズ推定値を提供した。我々はポーズ推定値をＩＭＵ測定値と融合させ、非一次（非線形）トラッキングコントローラのためのフィードバックとして直接的に使用される１００Ｈｚのステート推定値を発生させるためにアンセンテッドカルマンフィルタ（ＵＫＦ）を採用した。地上ステーションでは、ステレオベースのビジュアルＳＬＡＭモジュールが、高レベルプランナー（計画者）のために３Ｄボクセルグリップマップを発生させた。このＳＬＡＭモジュールは全地球ポーズ補正値も提供する。しかし、我々は、このポーズ補正値をビジョンベースのステート推定値と直接的には融合しなかった。なぜなら、そうすると、大きなループ閉鎖の際に重大なポーズ断絶が発生する可能性があったからである。その代わりに、我々は、もし飛行体がそれら変更された中間地点に従うなら、それでも飛行体がグローバル目標地点に到達できるように、そのポーズ補正値を使用して中間地点を変更した。我々はさらに、搭載されている高レベルコンピュータを１００Ｈｚで運用し、所望の中間地点を滑らかな多項飛行経路に変換させる飛行経路発生装置を開発した。

ＩＩＩ．ビジュアル慣性（ＶＩＮＳ）ステート推定

Ａ．ビジョンベースポーズ推定
我々は、飛行体の６−ＤＯＦポーズを推定するために我々の先の成果［文献１１］の改良を活用した。我々はプラットフォームに２つのカメラを装備させたが、我々は旧来のステレオベースステート推定を実行しなかった。事実、我々は主カメラとして、２０Ｈｚで画像を撮る１台のカメラをセットし、他方のカメラは１Ｈｚで画像を撮るように設計した。我々は高レート不等計算を実行しなかったので、必要な計算力は低下していた。しかし、そのステレオ形態は我々に、局所マップとポーズ推定値のスケールを保存する数量的情報を推定させた。

１）単眼ベースポーズ推定：
主要な魚眼カメラによって撮影される画像のために、我々はＦＡＳＴコーナを検出し［文献１２］、ＫＬＴトラッキング装置を使用してそれらをトラッキングした［文献１３］。主要カメラの高フレームレートのおかげで、我々は歪められた魚眼画像に対して直接的にトラッキングを実行することができ、画像忠実性に対するオーバヘッドの追加の計算を回避できた。我々は、ジャイロスコープ測定の短期一体化からの増分的回転推定値を利用し、トラッキングの孤立値を拒絶するために２ポイントＲＡＮＳＡＣを実行した。我々は、まだ三角測量されていない遠方の特徴部を活用するために、分離された配向性と位置推定スキームを提案する。ドリフトを最少とすべく、ルックバックヒストリでロボットＲ_ｊの配向性がエピ極性拘束を介して推定された。三角測量された３Ｄ特徴部ｐ_ｉ、ｉ∈Ｉで成る完全な３Ｄローカルマップの存在を想定すれば、ロボットｒ_ｊの位置は以下の一次系を解くことで効率的に見つけられる。
ここで、ｕ_ｉｊは単位長特徴部観察ベクトル；
と
６−ＤＯＦポーズが見つけられると、特徴部ｐ_ｉの位置は次の一次系を解くことで見つけられる。

ここで、Ａ_ｉｊとｂ_ｉｊはｉ番目の特徴部のｊ番目フレームまでの全観測値を表わす。これは無メモリ問題であり、よって、特徴部三角測量の複雑性は、その特定特徴部の観察値の数には関係なく一定である。

２）ステレオベーススケール回復：
前述のポーズ推定アプローチは、単眼ベース三角測量の蓄積された誤差によってスケールドリフトの悪影響を受ける。全瞬間ステレオ（立体）測定値がスケールドリフト補正のために活用される。Ｋを両方のカメラによって見られた特徴部のセットとする。我々は平均場面深度の相違を以下の式で計算できる。
ここで、
は、ステレオ三角測量だけを介して得られた３Ｄ特徴部の位置である。次に我々は、式（４）のごとくにｂ_ｉｊを修正することでスケールドリフトを補正し、式（２）を再び解くことができる。

Ｂ．ＵＫＦベースセンサ融合
ビジョンシステムからの２０Ｈｚポーズ推定のみではロボットを制御するのには十分ではない。遅延測定値補正でのＵＫＦが、１００Ｈｚでのロボットのポーズおよび速度を推定するのに使用された［文献１４］。このシステムのステートは次で定義される。
ここで、ｒはロボットの３Ｄ位置であり、ｑはロボットの３Ｄ配向性の四元数であり、ａ_ｂはボディフレーム内の加速度計測定値のバイアス（偏り）である。我々は、システムステートをプロパゲーションさせるために従来のＩＭＵベースプロセスモデルと、ステート訂正のために６−ＤＯＦポーズで成る一次測定値モデルとを使用する。

Ｃ．ビジュアル慣性ステートエスティメータ性能
図１６は、定義済み飛行経路の自律トラッキング中のバイコンモーションキャプチャシステム^２からのグランドツルースに対するＶＩＮＳエスティメータの性能の比較を示す（セクションＶＩＩＩ−Ａ）。搭載速度推定値は、標準偏差が
＝｛０．０５００，０．０７０６，０．０３０９｝（ｍ／秒）でバイコン推定値（全てボディフレームに変換）にほぼ匹敵した。しかし、ＶＩＮＳエスティメータのグローバルバンドル（一括）調節の欠如は、帰納（回帰）式によって、推定されたポーズの長期ドリフトの結果を招いた。よって我々は、そのようなドリフト現象を表わすためにオドメトリフレーム
を導入した。

ＩＶ．ビジュアルＳＬＡＭ
我々はＶＩＮＳシステムのドリフトを排除するためにビジュアルＳＬＡＭモジュールを利用した。ビジュアルＳＬＡＭは広く研究された分野である。小さな活用スペースにおいては、回帰フィルタリング［文献１５］または平行トラッキングおよびマッピング技術［文献１６］を使用するアプローチは正確な結果を提供する。単眼カメラ［文献１７］またはステレオカメラ［文献１８］による大きなスケールのマッピングは、ポーズグラフベース式を使用して達成される。我々のシステムにおいては、制限された搭載計算リソース、制限された無線伝送バンド幅および搭載エスティメータの精度によって、高レートビジュアルＳＬＡＭは不要であり、実行不可能である。従って、我々のビジュアルＳＬＡＭモジュールは１Ｈｚの最大レートにて機外で運用される。ポーズグラフベースＳＬＡＭの後端は、ワイドなベースラインループ閉鎖検出のためのＳＵＲＦ特徴部［文献１９］を利用する前端と共に、そのような低レートにて安定した性能を発揮する。我々は、ポーズグラフのためのノードを発生させるべく推定されたロボットの飛行経路をほとんどサンプリングしなかった。それぞれのノードのために、我々はステレオ画像間のＳＵＲＦ特徴部を検出してマッチングさせることで散在する３Ｄポイントを計算処理した。高密度の不同画像および高密度のポイントクラウドも計算された。

我々は、現在ノードの不確実長円の内側に入るノードをチェックすることでループ閉鎖を検出した。我々は、潜在的なループ閉鎖のために最初の候補から始めて一定数のノードをチェックした。２つの画面間の類似性を試験するためにＳＵＲＦ特徴部が使用された。我々は、ＲＡＮＳＡＣ ＰｎＰ［文献２０］を使用して現在ノードとループ閉鎖候補との間の相対変換を計算した。セクション３．４［文献２１］で提案される安定性試験が実行され、ループ閉鎖変換の形状的一貫性が確認された。形状検証をパスする候補変換がポーズグラフに加えられた。最後に、我々はポーズグラフ最適化のためにｉＳＡＭライブラリを使用した［文献２２］。最適化されたポーズグラフが見つけられると、我々は、高密度ポイントクラウドをグローバルフレームに投射することで３Ｄボクセルグリップマップを構築できた。このマップは高レベルプラニングのために使用され（セクションＶ）、実験の進行を人間のオペレータにモニターさせることができた。最適化されたポーズは、ワールドフレームの推定値を表わし、
で表わされる。
オドメトリフレームと世界フレームとの間の変換として機能するビジュアルＳＬＡＭ
からのポーズ訂正が以下のように式化された：
ここで、
は文献［２３］で定義されているポーズ更新関数である。従来のアプローチとは異なり、我々は、ＶＩＮＳシステムのドリフトを訂正するためにグローバルポーズ測定値として
を使用しなかった。その代わりに我々は、
を飛行経路発生装置に供給し（セクションＶＩ）、ループ閉鎖によって、ビジュアルＳＬＡＭポーズ推定値に大きな断絶が存在しても（すなわち、
が大きくても）、スムーズであることが保証される飛行経路を計算した。これは存在するアプローチからの我々のシステムの大きな離脱であり、複雑な環境内での高速自律ナビゲーションを可能にする鍵である。さらなる詳細はセクションＶＩに提供されている。

Ｖ．高レベルプラニング
我々は２段階プラニングアプローチを採用した。さらに高いレベルでは、世界フレームのユーザ特定中間地点が与えられ、クアドロータをシリンダとして扱うとすれば、所望の３Ｄ位置と偏飛行角の配列で成り、現在ロボット位置と所望ゴールを接続する高レベル経路（パス）は、オープンモーションプラニングライブラリ（ＯＭＰＬ）［文献２５］で実行されているように、ＲＲＴ^＊［文献２４］を使用して発生された。得られたパスは最低数のＫ中間地点
に簡素化され、さらなる改善のために飛行経路発生手段（セクションＶＩ）に送られた。このパスはマップ更新として同じ周波数での衝突の可能性のためにチェックされた（１Ｈｚ、セクションＩＶ）。高レベルプランナー（計画機）は中庸的な計算リソースを必要とするだけであるが、我々は、高レベルプラニングのために必要な全情報が機外のビジュアルＳＬＡＭモジュールから入ってくるので、それを機外で運用した。我々はまた、ユーザにプラナーをバイパスさせ、中間地点の配列を明確にセットさせた。

ＶＩ．飛行経路の発生
我々は、ビジュアルＳＬＡＭ（式６）からの最新ポーズ訂正を使用してオドメトリフレーム内に高レベルプランナー（計画機）からの全中間地点をまず変換した。
フィードバック制御のために、オドメトリフレームのステート推定値を使用してロボットが全ての変換された中間地点を通過して飛行するなら、それは世界フレームの同じセットの中間地点も通過して飛行するであろう。さらに、もし大きなスケールのループ閉鎖が存在すれば（すなわち
に大きな変化が存在すれば）、ロボットが向かっている中間地点のセットは大きく変化するであろう。しかしながら、もし我々がロボットの現在ステートに等しい当初条件でスムーズな飛行経路を再発生できるなら、飛行経路間の移行はスムーズになり、搭載ステートエスティメータおよびコントローラ内に特別な操作は不要であろう。

我々は、信頼できるステート推定値を同時に維持しながら、クアドロータが全ての中間地点をスムーズに通過することが確実になることを願った。ビジョンベース推定値の質を決定する重要な条件はトラッキング性能である。我々の魚眼カメラ構成で、図１７から、
大きな視認フィールドのため、急速平行移動がトラッキング性能にほとんど影響を及ぼさないことが理解されるであろう。しかし、急速回転は画像を簡単にピンボケさせ、ＫＬＴトラッカーの不調状態を引き起こすことがある。この観察は、横揺れ、および縦揺れの角速度を最少にする飛行経路を我々に設計させた。

クアドロータの動作の方程式を微分することで［文献２６］、ボディフレームの角速度は急動的な線形加速の導関数に密接に関係することが確認できた。よって、我々は水平方向のクアドロータの急動を最小化する飛行経路を発生させた。

垂直方向のために、我々はモータのＲＰＭの変化を最少にすることを願った。これも急動に対応する。高レベルパスからの大きな偏差を回避するために、この中間地点を接続する２つの線状セグメント間の角度が閾値を超えるならば、中間地点が１つの中間地点の直前および直後に加えられた。我々は、１０ｍｓ程度の実行時間でロボットを操作する搭載された多項的飛行経路発生アルゴリズム［文献２７］を利用した。最良の飛行経路は、次の非拘束２次方程式プログラミングを解くことで見つけられる：
ここで、ｙはそれぞれの中間地点での所望導関数値の集合であり、非固定あるいは固定のものでよい。我々は、再プラニングおよびループ閉鎖中にスムーズな飛行経路を維持するために、最初の中間地点で位置、速度、加速度をロボットの現在ステートとなるように固定した。最後の中間地点のためには速度および加速度はゼロにセットされた。その他の全ての中間地点のためには、位置のみが固定され、飛行経路発生手段は速度と加速度のプロフィールを提供した。多項性飛行経路の係数ｓは線形マッピングｓ＝Ｍｙを介して見つけられる。

上述の飛行経路発生アプローチの制限要件は、中間地点間の移動時間を予め定義する必要性である。計算の制約のため、我々は、最良のセグメント時間を見つけるために反復的時間最良化［文献２７、２８］を実行する代わりに、セグメントの始めと終わりに一定の加速度で常にゼロ速度から加速してゼロ速度に減速し、セグメントの中間で一定の速度を維持する線形飛行経路としてセグメント時間を近似する発見法を利用した。この単純な発見法は、短いセグメント中の過剰な加速を回避するのに役立ち、長いセグメントにとっては合理的な時間近似法である。

図１８は一連の中間地点から発生されたスムーズな飛行経路のクアドロータトラッキングをシミューレーションしたものである。中間地点の変更および飛行経路再発生は２０秒で実行された。再発生された飛行経路は当初飛行経路をスムーズに接続し、クアドロータはスムーズに中間地点を交替させることができた。

ＶＩＩ．制御
Ａ．位置トラッキングコントローラ
この業績のため、我々は、大きな角度変化および大きな加速が関与する高度に動力学的な動作におけるその優れた性能のために、非線形（非一次）的誤差測定性能を備えた位置トラッキングコントローラ［文献２９］の使用を選択した。ＶＩＮＳシステム（セクションＩＩＩ）からの１００Ｈｚステート推定値がコントローラのためのフィードバックとして直接的に使用された。我々の実施形態では、飛行姿勢コントローラはロボットのオートパイロットボードのＡＲＭプロセッサにて１ｋＨｚで運用され、位置トラッキング制御はアトムプロセッサにて１００ｋＨｚで操作された。

Ｂ．ハイブリッドシステムコントローラ
我々の目標は完全に自律した飛行体を開発することであるが、実験のある時点で、人間のオペレータが、飛行体の単純であるが直接的な制御を希望するかもしれない。よって、我々は、人とロボットの相互作用を許容する有限ステートマシンベースハイブリッドシステムコントローラ（図１９）を開発した。このコントローラには４つのモードが存在し、セクションＶＩＬＡのコントローラは位置モードで運用される。任意の時に、オペレータは遠隔モードを介して入力を伝送できる。これらコマンドは飛行体によって運動速度コマンドとして解釈される（ホバリングステートとなるコマンドが存在しない場合）。我々は、１Ｈｚの画像とグローバル３Ｄマップのみを使用し、直接的な視線を介在させずに、不慣れなオペレータが飛行体を制御できるという意味合いにおいて速度制御モードが容易に使用できることを実験的に試した。ホバリングモードはアイドリングステートとして機能し、飛行体はオペレータからのコマンドを待つ。

ＶＩＩＩ．実験結果
我々は、提案システムの性能を実証するために３つの代表的な実験を提示する。第１の実験は、グローバルに一貫したトラッキングを維持するための提案システムの能力を実証した。我々は、性能を定量化するためにグランドツルースとの比較を提供する。第２の実験では、ロボットが大きなループ（約１９０ｍ）を備えた屋内環境をナビゲーションし、１回のバッテリ充電内でループを完成させた（５分未満の飛行時間）。最後に、我々は、環境変化および強力な風の妨害に対する提案システムの安定性を強調する屋外ナビゲーション実験を提示する。

Ａ．グランドツルース比較によるシステム性能評価
この実験において、ロボットは約１ｍ／ｓの速度で方形パターンから発生されたスムーズな飛行経路に沿って飛行した。バイコンからのグランドツルースが使用され、グローバルトラッキング性能が定量化された。図２０（ａ）と図２０（ｂ）に見られるように、ＶＩＮＳステート推定値には遅い位置ドリフトが存在する。しかし、機外ビジュアルＳＬＡＭからのグローバル補正は、グローバルに一貫したオペレーションをもたらす。グローバルループ閉鎖はシステムに明確に統合されているが、ロボットはＶＩＮＳステート推定値を使用して制御されることに留意。ビジュアルＳＬＡＭから訂正によって、オドメトリフレームの所望のスムーズな飛行経路は継時的に再発生および変化する。図２０（ｂ）から、ロボットの実際の位置は所望の位置に収束し、標準偏差
＝｛０．１１０８，０．１１８６，０．０８３４｝（ｍ／ｓ）を有し、トラッキングがグローバルに一貫していることが理解できるであろう。

Ｂ．大ループを備えた屋内環境ナビゲーション
我々は、ループを備えた大スケールの環境を通過してロボットが自律的にナビゲーションする場合を考察する。ループのサイズ（約１９０ｍ）と、短いバッテリ寿命サイクル（５分未満）によって、その任務を完遂するために、我々は高速ナビゲーションを達成しなければならない。この環境は、無特徴床部（図２１）によって、下方に面したカメラを使用するアプローチ［文献６、７］に対して重大な挑戦を提起した。しかしながら、提案システムによって信頼できるステート推定値が得られ、ロボットは最大速度１．５ｍ／ｓ、平均速度１ｍ／ｓで成功裏に実験を完了した。大スケールのループ閉鎖は２５７秒で検出された（図２２（ｃ））。その間、ＳＬＡＭポーズと３Ｄマップの両方が大きく変化した（図２２（ａ）と図２２（ｂ））。しかし、図２２（ｃ）で見られるように、ロボットの
フィードバック制御のために使用されたステート推定値は実験を通してスムーズな状態を保ち、オドメトリフレームの変換された中間地点を辿ることでロボットはグローバル始点に戻ることができた（セクションＶＩ）。

Ｃ．複雑屋外環境における自律ナビゲーション
この実験は、屋内環境における提案システムの性能を立証する。実験はペンシルベニア州フィラデルフィア市において、ある典型的な冬日に実施された。そこでは風速が２０ｋｍ／時まで高まった。総航行距離は約１７０ｍであり、総飛行時間は１６６秒（図２３）であった。ビデオカメラからのスナップショットおよび搭載カメラで撮られた画像は図２４に示されている。この屋外環境はほぼ構造物がなく、立木と植物で構成されており、非建造物環境において運行できるシステムの性能が立証された。

ＩＸ．結論および将来成果
ここで説明しているように、我々は、搭載カメラおよびセンサとしてのＩＭＵだけを使用して軽量の既製クアドロータによる複雑環境内でのグローバルに一貫した自律ナビゲーションを可能にするシステムのデザインを提案する。我々は、急激な中間地点変更やループ閉鎖等の困難な状況時にスムーズな飛行経路のトラッキングを維持する課題に対処する。提案システムの性能を立証するために屋内環境および屋外環境の両方におけるオンライン実験結果が提示されている。

運行環境を拡張し、システムの安定性を増強するため、一体的なレーザ及び／又はＧＰＳベースのステート推定アプローチを我々の現在システムに組み込むことができる。

以下の参考文献それぞれの開示内容は全体的に本願に組み入れられている。

引用文献
［１］Ａ．バカラッチ、Ｓ．プレンティス、Ｒ．ヒーおよびＮ．ロイ「ＧＰＳ否定環境でのＲＡＮＧＥ安定自律ナビゲーション」実地ロボット工学学会誌、第２８巻５号、６４４６６６ページ、２０１１年
［２］Ｓ．グルゾンカ、Ｇ．グリセッティおよびＷ．バーガード「完全自律屋内クアドロータ」ＩＥＥＥ、輸送ロボット、ＰＰ巻９９号、１から１１ページ、２０１１年
［３］Ｓ．シェン、Ｎ．ミッチェルおよびＶ．クマール「コンピュータ的に規制されたＭＡＶでの自律複床屋内ナビゲーション」ロボットおよび自律に関するＩＥＥＥ国際会議、会報、上海市、中国、２０１１年５月、２０〜２５ページ
［４］Ｓ．シェラ、Ｊ．レフダ、Ｓ．アチャ、Ｈ．カバー、Ａ．チャンバーズ、Ｓ．ヌスケおよびＳ．シン「飛行ロボットからの河川マッピング：河川検出および障害物マッピング」自律ロボット、第３３巻１−２号、１８９〜１４ページ、２０１２年８月
［５］Ａ．クシュレイエブ、Ｂ．マクアリスタおよびＭ．リクハチェブ「空中備品配達における着地点選択のためのプラニング」ＩＥＥＥ／ＲＳＪ知的ロボットおよびシステム国際会議、会報、サンフランシスコ市、カルフォルニア州、２０１１年９月、１１４６〜１１５３ページ
［６］Ｓ．ワイズ、Ｍ．Ｗ．アキテリック、Ｓ．ライネン、Ｍ．チリおよびＲ．シーグワート「未知の環境におけるリアルタイム搭載ビジュアル慣性ステート推定およびＭＡＹＳの自己キャリブレーション」ロボットおよび自律に関するＩＥＥＥ国際会議、会報、セントポール市、ミネソタ州、２０１２年５月、９５７〜９６４ページ
［７］Ｆ．フランドルファ、Ｌ．ヘング、Ｄ．ホネガー、Ｇ．Ｈ．リー、Ｌ．メイア、Ｐ．タンスカネンおよびＭ．ポレフェイズ「クアドロータＭＡＶを使用したビジョンベース自律マッピングおよび探検」知的ロボットおよびシステムに関するＩＥＥＥ／ＲＳＪ国際会議、会報、ビラモウラ、アルガルベ市、ポルトガル国、２０１２年１０月
［８］Ａ．Ｓ．ファン、Ａ．バクラッチ、Ｐ．ヘンリ、Ｍ．クライニン、Ｄ．マツラナ、
Ｄ．フォックスおよびＮ．ロイ「ＲＧＢ−Ｄカメラを使用した自律飛行のためのビジュアルオドメトリおよびマッピング」ロボット研究の国際シンポジウム、会報、フラッグスタッフ市、アリゾナ州、２０１１年８月
［９］Ｃ．ビルズ、Ｊ．チェンおよびＡ．サクセナ「単一の画像眺望キューを使用した屋内環境での自律ＭＡＶ飛行」ロボットおよび自律に関するＩＥＥＥ国際会議、会報、上海市、中国、２０１１年５月、５７７６〜５７８３ページ
［１０］Ｇ．ドクルーン、Ｃ．Ｄ．ワグターブ、Ｂ．レメスブおよびＲ．ルイジュシンクブ「サブサンプリング：微小飛行体のためのリアルタイムビジョン」ロボットおよび自律システム、第６０巻２号、１６７から８１ページ、２０１２年２月
［１１］Ｓ．シェン、Ｙ．ムルガオンカ、Ｎ．ミッチェルおよびＶ．クマール「複雑環境における自律回転体ＭＡＶのためのビジョンベースステート推定」ロボットおよび自律に関するＩＥＥＥ国際会議、会報、カールスルーヒ市、ドイツ国、２０１３年５月、発行予定
［１２］Ｅ．ロステンンおよびＴ．ドルモンド「高速コーナ検出のためのマシン学習」コンピュータビジョンに関する欧州会議、会報、グラツ市、オーストリア国、２００６年５月
［１３］Ｂ．Ｄ．ルーカスおよびＴ．カナデ「ステレオビジョンへの適用を伴う反復画像レジストレーション技術」人工知能に関する国際共同会議、会報、バンクーバ市、カナダ国、１９８１年８月、２４〜２８ページ
［１４］Ｒ．Ｖ．Ｄ．マーウィ、Ｅ．Ａ．ワンおよびＳ．Ｉ．ジュリエール「非線形推定のためのシグマポイントカルマンフィルタ：一体化ナビゲーションへの適用」ＡＩＡＡ（米国航空宇宙工学協会）ガイダンス、ナビゲーションおよび制御に関する会議、会報、プロビデンス市、ロードアイランド州、２００４年８月
［１５］Ｊ．シベラ、Ａ．Ｊ．ダビソンおよびＪ．モンティエル「単眼ＳＬＡＭのための逆深度パラメータ化」ＩＥＥＥ、輸送ロボット、第２４巻、５号、９３２〜９４５ページ、２００８年１０月
［１６］Ｇ．クレインおよびＤ．ムレイ「小型ＡＲ作業領域のための平行トラッキングおよびマッピング」混成および増大された現実（ＩＳＭＡＲ’０７）に関する第６回ＩＥＥＥおよびＡＣＭ国際シンポジウム、会報、奈良市、日本国、２００７年１１月
［１７］Ｈ．ストラスダット、Ｊ．Ｍ．Ｍ．モンティエルおよびＡ．Ｊ．ダビソン「スケールドリフト認識大スケール単眼ＳＬＡＭ」ロボット：科学およびシステム、会報、ザラゴザ市、スペイン国、２０１０年６月
［１８］Ｃ．メイ、Ｇ．シブレイ、Ｍ．カミングズ、Ｐ．ニューマンおよびＩ．レイド「ＰＳＬＡＭ：ステレオを使用した定時での大スケールマッピングのためのシステム」コンピュータビジョンの国際学会誌、１〜７ページ、２０１０年６月
［１９］Ｈ．ベイ、Ｔ．ツイテラースおよびＬ．Ｖ．グール「ＳＵＲＦ：加速された安定特徴」コンピュータビジョンに関する欧州会議、会報、グラツ市、オーストリア国、２００６年５月
［２０］Ｆ．モレノ−ノグエル、Ｖ．レペチートおよびＰ．フュア「ＰｎＰ問題に対する高精度非反復０（ｎ）解」コンピュータビジョンに関するＩＥＥＥ国際会議、会報、リオデジャネイロ市、ブラジル国、２００７年１０月
［２１］Ｅ．Ｂ．オルソン「安定した効率的ロボットマッピング」博士論文、ＭＩＴ、ケンブリッジ市、マサチューセッツ州、２００８年６月
［２２］Ｍ．カエス、Ａ．ランガネーサンおよびＦ．デラート「ｉＳＡＭ：増分的スムーズ化およびマッピング」ＩＥＥＥ、輸送ロボット、第２４巻６号、１３６５〜１３７８ページ、２００８年１２月
［２３］Ｒ．スミス、Ｍ．セルフおよびＰ．チーズマン「ロボット工学における不確定空間関係の推定」ロボットおよび自律に関するＩＥＥＥ国際会議、会報、第４巻、ラレイ市、ノースカロライナ州、１９８７年３月、８５０ページ
［２４］Ｓ．カラマンおよびＥ．フラゾリ「最良モーションプラニングのための増分的サ
ンプリングベースアルゴリズム」ロボット；科学およびシステム、会報、ザラゴザ市、スペイン国、２０１０年６月
［２５］Ｉ．Ａ．スキャン、Ｍ．モールおよびＬ．Ｅ．カブラキ「オープンモーションプラニングライブラリ」ＩＥＥＥ、ロボットおよび自律学会誌、第１９巻４号、７２〜８２ページ、２０１２年１２月
http://ompl.kavrakilab.org
［２６］Ｎ．マイケル、Ｄ．メリンジャ、Ｑ．リンゼイおよびＶ．クマー「ＧＲＡＳＰマルチ微小ＵＡＶテスト床」ＩＥＥＥ、ロボットおよび自律学会誌、第１７巻３号、５６〜６５ページ、２０１０年９月
［２７］Ｃ．リヒタ、Ａ．ブライおよびＮ．ロイ「クアドロータ飛行のための多項飛行経路プラニング」ロボットおよび自律ＩＥＥＥ国際会議、会報、カールスルーヘ市、ドイツ国、２０１３年５月、出版予定
［２８］Ｄ．メリンジャおよびＶ．クマー「クアドロータのための最少スナップショットによる飛行経路発生および制御」ロボットおよび自律ＩＥＥＥ国際会議、会報、上海市、中国、２０１１年５月、２５２０〜５２５ページ
［２９］Ｔ．リー、Ｍ．レオキおよびＮ．マクラムロック「ＳＥ（３）上のクアドロータＵＡＶの地形的トラッキング制御」決定および制御国際会議、会報、アトランタ市、ジョージア州、２０１０年１２月、５４２０〜５４２５ページ

ここで説明した主題は、組み合わせが明示的に説明されていなくとも、ここで説明した要素または技術の全ての組み合わせを含む。例えば、“クアドロータでの複雑環境におけるビジョンベース自律ナビゲーション”と表題されたセクションで説明されている要素または方法論は、そのセクションに先立って説明されている任意の方法または要素と組み合わせることができる。

ここで説明されている主題の様々な詳細部は、ここで説明されている主題のスコープから逸脱せずに変更が可能である。さらに、前述の説明は、説明のみが目的であり、限定を意図したものではない。

Claims (16)

1. 屋内環境および屋外環境における飛行体の自律制御を可能にするシステムであって、
   センサからの現在および以前の測定値、並びに飛行体の以前の推定された状態が与えられると、前記飛行体の現在の状態を推定すべく、異なる様式の複数のセンサからの測定値を組み合わせるためのセンサ融合モジュールを含んでおり、
   該センサ融合モジュールは、１以上のセンサが不正確な情報を提供したとき、全地球測位システム（ＧＰＳ）測定値が利用可能期間後に利用できなくなったとき、あるいは、ＧＰＳ測定値が利用不能期間後に利用できるようになったとき、前記飛行体の状態の推定値の滑らかさを維持するように設計されており、本システムはさらに、
   推定された現在の状態と、ユーザまたはさらに高いレベルの計画者によって提供されたゴールあるいは中間地点に基づいて前記飛行体の飛行経路を制御するための計画を発生させる飛行経路ジェネレータを含んでいる、
   ことを特徴とするシステム。
   
2. 前記センサは、慣性測定装置（ＩＭＵ）と、圧力高度計、磁力計、レーザスキャナ、カメラ、下方対面光学センサ、および全地球測位システム（ＧＰＳ）レシーバのうちの少なくとも１つとを含んでいる、請求項１記載のシステム。
   
3. 前記センサ融合モジュールは、異なる様式のセンサからの測定値を組み合わせるためにアンセンティッドカルマンフィルタ（ＵＫＦ）を使用するように設計されており、該センサ融合モジュールのソフトウェアを再設計することなく、センサの追加および削減を可能にしている、請求項１記載のシステム。
   
4. 前記センサ融合モジュールは、現在の相対測定値および前記フィルタ内の増大された過去の状態の複製を使用して、現在の状態を推定するように設計されている、請求項３記載のシステム。
   
5. 前記センサ融合モジュールは、思慮分別的に、前記フィルタから増大された状態を排除し、該フィルタに新規の増大された状態を追加するように設計されている、請求項３記載のシステム。
   
6. 前記センサ融合モジュールは、前記フィルタに順番を乱して到着する前記センサからの測定値を融合するように設計されている、請求項３記載のシステム。
   
7. 屋内環境および屋外環境における飛行体の自律制御を可能にする方法であって、
   センサからの現在の測定値、並びに飛行体の以前の推定された状態が与えられると、前記飛行体の現在の状態の推定値を発生すべく、異なる様式の複数のセンサからの測定値を組み合わせるステップと、
   推定された現在の状態と、ユーザまたはさらに高いレベルの計画者によって入力されたゴールあるいは中間地点に基づいて前記飛行体の飛行経路を計画するための信号を発生させるステップと、
   １以上の前記センサからの出力が不正確であるとき、全地球測位システム（ＧＰＳ）測定値が利用不能期間後に利用できるようになったとき、あるいは、ＧＰＳ測定値が利用可
   能期間後に利用できなくなったとき、前記飛行体の状態の変化を平滑化するステップと、を含んでいることを特徴とする方法。
   
8. 前記センサは、慣性測定装置（ＩＭＵ）と、圧力高度計、磁力計、レーザスキャナ、カメラ、下方対面光学センサ、および全地球測位システム（ＧＰＳ）レシーバのうちの少なくとも１つとを含んでいる、請求項７記載の方法。
   
9. 前記測定値を組み合わせるステップは、異なる様式のセンサからの測定値を組み合わせるためにアンセンティッドカルマンフィルタ（ＵＫＦ）を使用し、センサ融合モジュールを再設計することなく、センサの追加および削減を可能化にすることを含んでいる、請求項７記載の方法。
   
10. 現在の状態の推定は、現在の相対測定値および前記フィルタ内の増大された過去の状態の複製の使用を含んでいる、請求項９記載の方法。
    
11. 新規の増大された状態の追加に応じて、フィルタから増大された状態を排除することを含んでおり、バイナリ選択行列は以前の増大された状態の追加に対応している、請求項９記載の方法。
    
12. フィルタに順番を乱して到着するセンサからの測定値を融合することを含んでいる、請求項９記載の方法。
    
13. コンピュータのプロセッサで実行されると、以下のステップを実施するように該コンピュータを制御する実行可能な指示を保存している非一時的コンピュータ可読媒体：
    センサからの現在の測定値、並びに飛行体の以前の推定された状態が与えられると、飛行体の現在の状態の推定値を発生すべく、異なる様式の複数のセンサからの測定値を組み合わせるステップ；
    推定された現在の状態と、ユーザまたはさらに高いレベルの計画者によって入力されたゴールあるいは中間地点に基づいて前記飛行体の飛行経路を計画するための信号を発生させるステップ；
    １以上のセンサからの出力が不正確であるとき、全地球測位システム（ＧＰＳ）測定値が利用不能期間後に利用できるようになったとき、あるいは、ＧＰＳ測定値が利用可能期間後に利用できなくなったとき、前記飛行体の状態の変化を平滑化するステップ。
    
14. 前記飛行体は、回転翼型小型飛行体（MAV）を含むことを特徴とする請求項１に記載のシ
    ステム。
    
15. 前記飛行体は、回転翼型小型飛行体（MAV）を含むことを特徴とする請求項７に記載の方
    法。
    
16. 前記飛行体は、回転翼型小型飛行体（MAV）を含むことを特徴とする請求項１３に記載の
    非一時的コンピュータ可読媒体。