(57)【要約】      （修正有）【課題】高精度な位置推定を行える移動体位置推定装置を提供する。【解決手段】取得部１０は、第１画像１１に関する第１データ１１ａと、移動体の移動に伴う第１画像１１との差異を含む第２画像１２に関する第２データ１２ａと、を含む情報を取得する。処理部２０は、情報に基づいて移動に伴う回転の方向を推定し、回転の方向に基づいて、第１画像内の第１領域ｒ１内の複数の第１特徴点と、第１画像内の第２領域ｒ２内の複数の第２特徴点と、を検出し、複数の第１特徴点の第１領域ｒ１内における第１密度を、複数の第２特徴点の第２領域ｒ２内における第２密度よりも高くし、第２画像内の第３領域ｒ３内の複数の第１対応点であって、複数の第１対応点のそれぞれは複数の第１特徴点の少なくともいずれかのそれぞれに対応する複数の第１対応点を求め、複数の第１特徴点のそれぞれと、複数の第１対応点のそれぞれと、に基づいて、移動体の位置の変化を推定する。【選択図】図１
2016061604.tif 000002
【特許請求の範囲】
【請求項１】
第１画像に関する第１データと、移動体の移動に伴う前記第１画像との差異を含む第２画像に関する第２データと、を含む情報を取得する取得部と、
前記情報に基づいて前記移動に伴う回転の方向を推定する方向推定処理と、
前記回転の方向に基づいて、前記第１画像内の第１領域内の複数の第１特徴点と、前記第１画像内の第２領域内の複数の第２特徴点と、を検出し、前記複数の第１特徴点の前記第１領域内における第１密度は、前記複数の第２特徴点の前記第２領域内における第２密度よりも高い検出処理と、
前記第２画像内の第３領域内の複数の第１対応点であって、前記複数の第１対応点のそれぞれは前記複数の第１特徴点の少なくともいずれかのそれぞれに対応する前記複数の第１対応点を求め、前記複数の第１特徴点のそれぞれと、前記複数の第１対応点のそれぞれと、に基づいて、前記移動体の位置の変化を推定する位置推定処理と、
を実施する処理部と、
を備えた移動体位置推定装置。
【請求項２】
前記回転の方向は、第１方向であり、
前記第１画像内において前記第２領域から前記第１領域に向かう第２方向は、前記第１方向とは逆の方向の成分を有する請求項１記載の移動体位置推定装置。
【請求項３】
前記第１画像と前記第２画像とを重ねたときに、前記第１領域から前記第３領域に向かう方向は、前記第２方向に沿う請求項２記載の移動体位置推定装置。
【請求項４】
前記第１方向は左方向及び右方向の一方であり、前記第２方向は左方向及び右方向の他方である請求項２または３に記載の移動体位置推定装置。
【請求項５】
前記第１方向は上方向及び下方向の一方であり、前記第２方向は上方向及び下方向の他方である請求項２または３に記載の移動体位置推定装置。
【請求項６】
前記回転の角度が定められた値以下のときは、
前記第２領域は、前記第１画像の中心部を含み、
前記第１領域は、前記第１画像内において前記第２領域の周りに設けられる請求項１記載の移動体位置推定装置。
【請求項７】
前記処理部は、前記検出処理の前に、前記第１領域の前記第１画像内の位置と、前記第２領域の前記第１画像内の位置と、を決める画像分割処理をさらに実施する請求項１〜６のいずれか１つに記載の移動体位置推定装置。
【請求項８】
前記方向推定処理は、前記第１画像と前記第２画像とに基づいて、前記回転の方向を推定することを含む請求項１〜７のいずれか１つに記載の移動体位置推定装置。
【請求項９】
前記情報は、前記移動体に搭載されたセンサにより得られた角速度に関する情報をさらに含み、
前記方向推定処理は、前記角速度に関する前記情報に基づいて、前記回転の方向を推定することを含む請求項１〜７のいずれか１つに記載の移動体位置推定装置。
【請求項１０】
前記情報は、前記移動体に搭載されたセンサにより出力された角加速度に関する情報をさらに含み、
前記方向推定処理は、前記角加速度に関する前記情報に基づいて、前記回転の方向を推定することを含む請求項１〜７のいずれか１つに記載の移動体位置推定装置。
【請求項１１】
第１画像に関する第１データと、移動体の移動に伴う前記第１画像との差異を含む第２画像に関する第２データと、を含む情報を取得する取得部と、
前記情報に基づいて前記移動に伴う前記移動体の位置の変化の方向を推定する方向推定処理と、
前記位置の変化の方向に基づいて、前記第１画像内の第１領域内の複数の第１特徴点と、前記第１画像内の第２領域内の複数の第２特徴点と、を検出し、前記複数の第１特徴点の前記第１領域内における第１密度は、前記複数の第２特徴点の前記第２領域内における第２密度よりも高い検出処理と、
前記第２画像内の第３領域内の複数の第１対応点であって、前記複数の第１対応点のそれぞれは前記複数の第１特徴点の少なくともいずれかのそれぞれに対応する前記複数の第１対応点を求め、前記複数の第１特徴点のそれぞれと、前記複数の第１対応点のそれぞれと、に基づいて、前記移動体の位置の変化を推定する位置推定処理と、
を実施する処理部と、
を備えた移動体位置推定装置。
【請求項１２】
前記位置の変化の方向は、前記移動体の前後方向であり、
前記第２領域は、前記第１画像の中心部を含み、
前記第１画像内において、前記第１領域は、前記第２領域の周りに設けられる請求項１１記載の移動体位置推定装置。
【請求項１３】
前記第１画像と前記第２画像とを重ねたときに、前記第１領域と前記第３領域とが重なる請求項１２記載の移動体位置推定装置。
【請求項１４】
前記方向推定処理は、前記第１画像と前記第２画像とに基づいて、前記位置の変化の方向を推定することを含む請求項１１〜１３のいずれか１つに記載の移動体位置推定装置。
【請求項１５】
前記情報は、前記移動体に搭載されたセンサにより得られた速度に関する情報をさらに含み、
前記方向推定処理は、前記速度に関する前記情報に基づいて、前記位置の変化の方向を推定することを含む請求項１１〜１３のいずれか１つに記載の移動体位置推定装置。
【請求項１６】
前記情報は、前記移動体に搭載されたセンサにより得られた加速度に関する情報をさらに含み、
前記方向推定処理は、前記加速度に関する前記情報に基づいて、前記位置の変化の方向を推定することを含む請求項１１〜１３のいずれか１つに記載の移動体位置推定装置。
【請求項１７】
前記第２画像は、前記第２領域に対応する第４領域をさらに含み、
前記位置推定処理は、前記第４領域内の複数の第２対応点であって、前記複数の第２対応点のそれぞれは前記複数の第２特徴点の少なくともいずれかのそれぞれに対応する前記複数の第２対応点を求め、前記複数の第２特徴点のそれぞれと、前記複数の第２対応点のそれぞれと、に基づいて、前記移動体の位置の変化を推定する請求項１〜１６のいずれか１つに記載の移動体位置推定装置。
【請求項１８】
前記第１画像及び前記第２画像は、前記移動体に搭載された撮像部で撮像された画像である請求項１〜１７のいずれか１つに記載の移動体位置推定装置。
【請求項１９】
第１画像に関する第１データと、移動体の移動に伴う前記第１画像との差異を含む第２画像に関する第２データと、を含む情報を取得する処理と、
前記情報に基づいて前記移動に伴う回転の方向を推定する処理と、
前記回転の方向に基づいて、前記第１画像内の第１領域内の複数の第１特徴点と、前記第１画像内の第２領域内の複数の第２特徴点と、を検出し、前記複数の第１特徴点の前記第１領域内における第１密度は、前記複数の第２特徴点の前記第２領域内における第２密度よりも高い処理と、
前記第２画像内の第３領域内の複数の第１対応点であって、前記複数の第１対応点のそれぞれは前記複数の第１特徴点の少なくともいずれかのそれぞれに対応する前記複数の第１対応点を求め、前記複数の第１特徴点のそれぞれと、前記複数の第１対応点のそれぞれと、に基づいて、前記移動体の位置の変化を推定する処理と、
を含む移動体位置推定方法。
【請求項２０】
第１画像に関する第１データと、移動体の移動に伴う前記第１画像との差異を含む第２画像に関する第２データと、を含む情報を取得する処理と、
前記情報に基づいて前記移動に伴う回転の方向を推定する処理と、
前記回転の方向に基づいて、前記第１画像内の第１領域内の複数の第１特徴点と、前記第１画像内の第２領域内の複数の第２特徴点と、を検出し、前記複数の第１特徴点の前記第１領域内における第１密度は、前記複数の第２特徴点の前記第２領域内における第２密度よりも高い処理と、
前記第２画像内の第３領域内の複数の第１対応点であって、前記複数の第１対応点のそれぞれは前記複数の第１特徴点の少なくともいずれかのそれぞれに対応する前記複数の第１対応点を求め、前記複数の第１特徴点のそれぞれと、前記複数の第１対応点のそれぞれと、に基づいて、前記移動体の位置の変化を推定する処理と、
をコンピュータに実行させる移動体位置推定プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明の実施形態は、移動体位置推定装置、移動体位置推定方法及び移動体位置推定プログラムに関する。
【背景技術】
【０００２】
移動体に搭載されたカメラ等で撮像された画像に基づいて、３次元空間上における移動体の位置を推定する移動体位置推定装置がある。移動体位置推定装置においては、周辺環境の変化に対して位置推定の高精度化が望まれている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平６−３１４３４０号公報
【特許文献２】特開２００７−７８４７６号公報
【特許文献３】特開２００９−１５６６１１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
本発明の実施形態は、高精度な位置推定を行える移動体位置推定装置、移動体位置推定方法及び移動体位置推定プログラムを提供する。
【課題を解決するための手段】
【０００５】
本発明の実施形態によれば、移動体位置推定装置は、取得部と、処理部と、を含む。前記取得部は、第１画像に関する第１データと、移動体の移動に伴う前記第１画像との差異を含む第２画像に関する第２データと、を含む情報を取得する。前記処理部は、前記情報に基づいて前記移動に伴う回転の方向を推定する方向推定処理と、前記回転の方向に基づいて、前記第１画像内の第１領域内の複数の第１特徴点と、前記第１画像内の第２領域内の複数の第２特徴点と、を検出し、前記複数の第１特徴点の前記第１領域内における第１密度は、前記複数の第２特徴点の前記第２領域内における第２密度よりも高い検出処理と、前記第２画像内の第３領域内の複数の第１対応点であって、前記複数の第１対応点のそれぞれは前記複数の第１特徴点の少なくともいずれかのそれぞれに対応する前記複数の第１対応点を求め、前記複数の第１特徴点のそれぞれと、前記複数の第１対応点のそれぞれと、に基づいて、前記移動体の位置の変化を推定する位置推定処理と、を実施する。
【図面の簡単な説明】
【０００６】
【図１】第１の実施形態に係る移動体位置推定装置を例示するブロック図である。
【図２】移動体、撮像部及び移動体位置推定装置を例示する模式図である。
【図３】第１の実施形態に係る移動体位置推定方法を例示するフローチャート図である。
【図４】画像分割部による画像分割処理を例示する模式図である。
【図５】図５（ａ）〜図５（ｃ）は、特徴点検出部での処理を例示する模式図である。
【図６】図５（ｂ）及び図５（ｃ）の例を説明する模式図である。
【図７】特徴点検出部での特徴点検出処理を例示するフローチャート図である。
【図８】任意回転での特徴点検出部の処理を例示する模式図である。
【図９】図９（ａ）及び図９（ｂ）は、位置推定部での処理を例示する模式図である。
【図１０】位置推定部での位置推定処理を例示するフローチャート図である。
【図１１】第２の実施形態に係る移動体位置推定装置を例示するブロック図である。
【発明を実施するための形態】
【０００７】
以下に、本発明の各実施の形態について図面を参照しつつ説明する。  
なお、図面は模式的または概念的なものであり、各部分の厚みと幅との関係、部分間の大きさの比率などは、必ずしも現実のものと同一とは限らない。また、同じ部分を表す場合であっても、図面により互いの寸法や比率が異なって表される場合もある。  
なお、本願明細書と各図において、既出の図に関して前述したものと同様の要素には同一の符号を付して詳細な説明は適宜省略する。
【０００８】
（第１の実施形態）
図１は、第１の実施形態に係る移動体位置推定装置を例示するブロック図である。  
移動体位置推定装置１００には、取得部１０と、処理部２０と、が設けられる。取得部１０には、例えば、入出力端子が用いられる。取得部１０は、有線または無線を介して外部と通信する入出力インターフェイスを含む。処理部２０には、例えば、ＣＰＵやメモリなどを含む演算装置が用いられる。処理部２０の各ブロックの一部、又は全部には、ＬＳＩ（Large Scale Integration）等の集積回路またはＩＣ（Integrated Circuit）チップセットを用いることができる。各ブロックに個別の回路を用いてもよいし、一部又は全部を集積した回路を用いてもよい。各ブロック同士が一体として設けられてもよいし、一部のブロックが別に設けられてもよい。また、各ブロックのそれぞれにおいて、その一部が別に設けられてもよい。集積化には、ＬＳＩに限らず、専用回路又は汎用プロセッサを用いてもよい。
【０００９】
取得部１０は、情報１３を取得する。情報１３は、第１データ１１ａと、第２データ１２ａと、を含む。第１データ１１ａは、第１領域ｒ１と第２領域ｒ２とを含む第１画像１１に関する。第２データ１２ａは、第１領域ｒ１に対応する第３領域ｒ３を含む第２画像１２に関する。なお、第２画像１２は、第２領域ｒ２に対応する第４領域ｒ４をさらに含んでいてもよい。第２画像１２は、移動体の移動に伴う第１画像１１との差異を含む画像である。例えば、第１画像１１は時刻ｔ０で撮像された画像であり、第２画像１２は時刻ｔ１（＞時刻ｔ０）で撮像された画像である。第２画像１２は時刻ｔ０で撮像された画像であり、第１画像１１は時刻ｔ１（＞時刻ｔ０）で撮像された画像である。第１画像１１と第２画像１２とは時系列的に異なる画像である。
【００１０】
処理部２０には、画像分割部２１と、方向推定部２２と、特徴点検出部２３と、位置推定部２４と、が設けられる。これらの各部は、例えば、移動体位置推定プログラムとして実現される。すなわち、移動体位置推定装置１００は、汎用のコンピュータ装置を基本ハードウェアとして用いることでも実現される。移動体位置推定装置１００に含まれる各部の機能は、上記のコンピュータ装置に搭載されたプロセッサに移動体位置推定プログラムを実行させることにより実現することができる。このとき、移動体位置推定装置１００は、上記の移動体位置推定プログラムをコンピュータ装置にあらかじめインストールすることで実現してもよいし、ＣＤ−ＲＯＭなどの記憶媒体に記憶して、あるいはネットワークを介して上記の移動体位置推定プログラムを配布して、この移動体位置推定プログラムをコンピュータ装置に適宜インストールすることで実現してもよい。また、処理部２０は、上記のコンピュータ装置に内蔵あるいは外付けされたメモリ、ハードディスクもしくはＣＤ−Ｒ、ＣＤ−ＲＷ、ＤＶＤ−ＲＡＭ、ＤＶＤ−Ｒなどの記憶媒体などを適宜利用して実現することができる。
【００１１】
図２は、移動体、撮像部及び移動体位置推定装置を例示する模式図である。  
移動体位置推定装置１００は、移動体４０の３次元空間上における位置の推定に用いられる。移動体４０としては、例えば、自動車やＵＡＶ(Unmanned Aerial Vehicle)などの移動体全般が対象とされる。移動体４０には、１つ以上の撮像部３０が搭載される。この撮像部３０の設置方法は特に限定されるものではない。撮像部３０は、例えば、撮像部３０の奥行き方向（撮像方向）が移動体４０の主進行方向と平行になるように設置される。撮像部３０は、撮像部３０の奥行き方向（撮像方向）が移動体４０の主進行方向と垂直になるように設置してもよい。
【００１２】
移動体位置推定装置１００は、例えば、撮像部３０と共に移動体４０に搭載される。移動体位置推定装置１００は、移動体４０とは別の場所に設けられていてもよい。つまり、移動体位置推定装置１００は、設置場所に係わらず、撮像部３０と通信可能に接続されていればよい。
【００１３】
以下、撮像部３０の奥行き方向が移動体主進行方向と平行に設置した場合について説明する。なお、１回目の画像取得時における撮像部３０の位置及び姿勢を基準とし、この基準に対して２回目以降の画像取得時における撮像部３０の位置及び姿勢が表される。ただし、基準の取り方はこの限りではなく、相対的な位置及び姿勢であればよい。
【００１４】
図３は、第１の実施形態に係る移動体位置推定方法を例示するフローチャート図である。  
取得部１０は、撮像部３０で撮像された第１画像１１と第２画像１２とを含む情報１３を取得する（ステップＳ１）。
方向推定部２２は、情報１３に基づいて、移動体４０の移動に伴う回転の方向を推定する方向推定処理を実施する（ステップＳ２）。
【００１５】
この例では、撮像部３０の位置及び姿勢の時系列的な変化を、移動体４０の位置及び姿勢の時系列的な変化とみなす。具体的には、撮像部３０で撮像される時系列的に異なる２つの画像が用いられる。２つの画像としては、例えば、第１画像１１及び第２画像１２が用いられる。すなわち、方向推定部２２は、第１画像１１及び第２画像１２に基づいて移動体４０の姿勢の変化を推定し、この姿勢の変化に基づいて、移動体４０の移動の回転の方向を推定する。
【００１６】
図１及び図２において、移動体４０の回転の方向が第１方向４０ａのときは、第１画像１１内において第２領域ｒ２から第１領域ｒ１に向かう第２方向４０ｂは、第１方向４０ａとは逆の方向の成分を有する。例えば、第１方向４０ａは左方向及び右方向の一方であり、第２方向４０ｂは左方向及び右方向の他方である。
【００１７】
図１の例では、移動体４０の回転方向が左方向となるため、第１方向４０ａが左方向、第２方向４０ｂが右方向となる。ここで、第１画像１１と第２画像１２とを重ねたときに、第３領域ｒ３が第１領域ｒ１よりも右側に位置することになる。すなわち、第１画像１１と第２画像１２とを重ねたときに、第１領域ｒ１から第３領域ｒ３に向かう方向は、第２方向４０ｂに沿っている。これより、２つの画像から回転方向を推定することができる。
【００１８】
方向推定部２２は、第１画像１１及び第２画像１２に基づいて移動体４０の位置の変化を推定し、この位置の変化に基づいて、移動体４０の移動の並進方向を推定してもよい。ここでいう並進方向とは、移動体４０の位置の変化の方向である。
【００１９】
並進方向の場合、第１画像１１と第２画像１２とを重ねたときに、第１領域ｒ１と第３領域ｒ３とが重なる。これより、２つの画像から並進方向を推定することができる。
【００２０】
図２に例示する座標系を、移動体４０に対して設定する。すなわち、移動体４０の前後（ロール：roll）軸５１をＺ軸とする。移動体４０の上下（ヨー：yaw）軸５２をＹ軸とする。移動体４０の左右（ピッチ：pitch）軸５３をＸ軸とする。これらＸ軸、Ｙ軸、Ｚ軸は、３次元空間内で互いに直交する。Ｘ軸方向に対して垂直な１つの方向をＹ軸方向とする。Ｘ軸方向に対して垂直でＹ軸方向に対して垂直な方向をＺ軸方向とする。左右軸５３（Ｘ軸）まわりの上下方向の回転角をピッチ角ｐとし、上下軸５２（Ｙ軸）まわりの左右方向の回転角をヨー角ｙとし、前後軸５１（Ｚ軸）まわりの回転角をロール角ｒとする。なお、座標系の取り方は一例である。
【００２１】
ある時刻ｔ０における撮像部３０の位置及び姿勢を、Ｐ０＝［Ｒ０｜ｔ０］で表す。時刻ｔ０と異なる時刻ｔ１における撮像部３０の位置及び姿勢を、Ｐ１＝［Ｒ１｜ｔ１］で表す。なお、Ｒ０及びＲ１は回転行列、ｔ０及びｔ１は並進ベクトルを表す。時刻ｔ０〜時刻ｔ１の相対的な位置及び姿勢の変化は、
ΔＲ＝Ｒ０＾ＴＲ１
Δｔ＝Ｒ０＾Ｔ（ｔ１−ｔ０）
と表せる。ここで、ΔＲは姿勢の変化、Δｔは位置の変化、＾Ｔは転置を意味する。
【００２２】
Δｔの成分のうち絶対値が最大となる方向を並進方向とする。例えば、Δｔ＝（０、０、１）であれば、＋ｚ方向が並進方向となる。Δｔ＝（−１、０、０）であれば、−ｘ方向が並進方向となる。ただし、この推定方法は一例であり、例えば、Δｔを方向ベクトルと考えた場合、各軸についての回転角度を８方向で量子化すると全体で５１２方向が定義できる。
【００２３】
ΔＲの各成分を、ロール・ピッチ・ヨー（ｒｏｌｌ、ｐｉｔｃｈ、ｙａｗ）の各成分に変換し、変換した各成分のうち絶対値が閾値以上となる方向を回転方向とする。例えば、閾値を３０としたとき、ΔＲ＝（１、５、−４５）であれば、−ｙａｗ方向が回転方向となる。ただし、この推定方法は一例であり、例えば、各成分について８段階で量子化すると全体で５１２方向が定義できる。
【００２４】
撮像部３０の位置及び姿勢の推定方法としては、例えば、ＳｆＭ(Structure from Motion)などの方法を用いることができる。これによれば、時系列的に異なる少なくとも２つの画像から撮像部３０の位置及び姿勢を推定できる。なお、この方法では、画像から特徴点を検出する必要がある。特徴点の検出は、後述の方法で実施することが可能である。
【００２５】
さらに、画像分割部２１は、第１領域ｒ１の第１画像１１内の位置と、第２領域ｒ２の第１画像１１内の位置と、を決める画像分割処理を実施する（ステップＳ３）。
【００２６】
図４は、画像分割部２１による画像分割処理を例示する模式図である。  
画像分割部２１は、第１画像１１を、複数の部分ｐ１〜ｐ９に分割する画像分割処理を実施する。例えば、画像平面上で縦３つ横３つ計９つの矩形部分に分割する。ただし、分割方法は一例であり、例えば、画像平面上で縦２つの矩形部分に分割する。例えば、中心が画像中央で直径が画像の高さの半分となる円部分とそれ以外の部分に分割する。例えば、画像中央から画像周辺に向かって放射状に分割する。さらに、３次元実空間における特徴点位置が得られる場合、画像奥行方向について部分に分割してもよい。例えば、撮像部３０からの距離が０〜１００メートルまで１０メートル間隔で３次元実空間を部分に分割できる。
【００２７】
さらに、特徴点検出部２３は、ステップＳ２で推定した回転の方向に基づいて、第１領域ｒ１内の複数の第１特徴点と、第２領域ｒ２内の複数の第２特徴点と、を検出する（ステップＳ４）。
【００２８】
図５（ａ）〜図５（ｃ）は、特徴点検出部２３での処理を例示する模式図である。  
図５（ａ）は、回転方向が前後軸５１回りである場合の処理を例示する図である。  
図５（ｂ）は、回転方向が上下軸５２の左回りである場合の処理を例示する図である。  図５（ｃ）は、回転方向が上下軸５２の右回りである場合の処理を例示する図である。
【００２９】
特徴点検出部２３は、方向推定部２２で推定した回転方向に基づいて、第１画像１１から複数の特徴点を検出する。具体的には、特徴点検出部２３は、上記の回転方向に基づいて、複数の第１特徴点ｃ１の第１領域ｒ１内における第１密度ｄ１が、複数の第２特徴点ｃ２の第２領域ｒ２内における第２密度ｄ２よりも高くなるように検出する検出処理を実施する。特徴点としては、画像上の特徴的な画素（例えば、コーナーやエッジなど）を検出する。例えば、Harrisのコーナー検出法や、Shi-Tomasiの方法、FAST特徴検出、SUSANオペレータなどの各種方法を用いることができる。なお、第１領域ｒ１の第１画像１１内での位置と、第２領域ｒ２の第１画像１１内での位置は、図４の画像分割位置に基づいて決定される。
【００３０】
図５（ａ）の例において、回転の方向が移動体４０の前後軸５１回り、つまり、回転の角度が定められた値以下のときは、移動体４０が直進していると推定される。この場合、第２領域ｒ２は、第１画像１１の中心部（例えば、ｐ２、ｐ５、ｐ８）を含む。第１画像１１内において、第１領域ｒ１は、第２領域ｒ２の周り（例えば、ｐ１、ｐ３、ｐ４、ｐ６、ｐ７、ｐ９）に設けられる。つまり、第１画像１１の左側部分（ｐ１、ｐ４、ｐ７）と右側部分（ｐ３、ｐ６、ｐ９）とが第１領域ｒ１となる。第１画像１１の中央部分（ｐ２、ｐ５、ｐ８）が第２領域ｒ２となる。複数の第１特徴点ｃ１の第１領域ｒ１内における第１密度ｄ１が、複数の第２特徴点ｃ２の第２領域ｒ２内における第２密度ｄ２よりも高くなるように検出される。
【００３１】
図５（ｂ）の例において、第１方向４０ａが左方向、第２方向４０ｂが右方向である。回転の方向が移動体４０の上下軸５２の左回り、つまり、回転の方向が左方向のときは、移動体４０が左折していると推定される。この場合、第１画像１１内において、第１領域ｒ１は、第２領域ｒ２の右側に位置する。この例では、第１画像１１の左側部分（ｐ１、ｐ４、ｐ７）及び中央部分（ｐ２、ｐ５、ｐ８）が第２領域ｒ２となる。第２領域ｒ２の右側部分（ｐ３、ｐ６、ｐ９）が第１領域ｒ１となる。同様に、複数の第１特徴点ｃ１の第１領域ｒ１内における第１密度ｄ１が、複数の第２特徴点ｃ２の第２領域ｒ２内における第２密度ｄ２よりも高くなるように検出される。
【００３２】
図５（ｃ）の例において、第１方向４０ａが右方向、第２方向４０ｂが左方向である。回転の方向が移動体４０の上下軸５２の右回り、つまり、回転の方向が右方向のときは、移動体４０が右折していると推定される。この場合、第１画像１１内において、第１領域ｒ１は、第２領域ｒ２の左側に位置する。この例では、第１画像１１の右側部分（ｐ３、ｐ６、ｐ９）及び中央部分（ｐ２、ｐ５、ｐ８）が第２領域ｒ２となる。第２領域ｒ２の左側部分（ｐ１、ｐ４、ｐ７）が第１領域ｒ１となる。同様に、複数の第１特徴点ｃ１の第１領域ｒ１内における第１密度ｄ１が、複数の第２特徴点ｃ２の第２領域ｒ２内における第２密度ｄ２よりも高くなるように検出される。
【００３３】
移動体４０が飛翔体などの場合、第１方向４０ａが上方向及び下方向の一方であり、第２方向が上方向及び下方向の他方としてもよい。つまり、回転の方向が移動体４０の左右軸５３の上回り、つまり、回転の方向が上方向のときは、移動体４０が上昇していると推定される。この場合、第１画像１１内において、第１領域ｒ１は、第２領域ｒ２の下側に位置する。図４の例では、第１画像１１の上側部分（ｐ１、ｐ２、ｐ３）及び中央部分（ｐ４、ｐ５、ｐ６）が第２領域ｒ２となる。第２領域ｒ２の下側部分（ｐ７、ｐ８、ｐ９）が第１領域ｒ１となる。同様に、複数の第１特徴点ｃ１の第１領域ｒ１内における第１密度ｄ１が、複数の第２特徴点ｃ２の第２領域ｒ２内における第２密度ｄ２よりも高くなるように検出される。
【００３４】
回転の方向が移動体４０の左右軸５３の下回り、つまり、回転の方向が下方向のときは、移動体４０が下降していると推定される。この場合、第１画像１１内において、第１領域ｒ１は、第２領域ｒ２の上側に位置する。図４の例では、第１画像１１の下側部分（ｐ７、ｐ８、ｐ９）及び中央部分（ｐ４、ｐ５、ｐ６）が第２領域ｒ２となる。第２領域ｒ２の上側部分（ｐ１、ｐ２、ｐ３）が第１領域ｒ１となる。同様に、複数の第１特徴点ｃ１の第１領域ｒ１内における第１密度ｄ１が、複数の第２特徴点ｃ２の第２領域ｒ２内における第２密度ｄ２よりも高くなるように検出される。
【００３５】
図６は、図５（ｂ）及び図５（ｃ）の例を説明する模式図である。  
図６は、移動体４０をＺＸ平面に投影した状態を表す。  
移動体４０に搭載された撮像部３０により画像４１が撮像される。移動体４０が移動(例えば、右折)した場合、この移動に伴い画像４１も矢印ａ１の方向に回転（ここでは右回転）する。この場合、移動体４０に対して、画像４１の左側が第１領域ｒ１となり、画像４１の右側が第２領域ｒ２となる。
【００３６】
ここで、移動体４０の移動の並進方向を推定する場合、特徴点検出部２３は、この並進方向に基づいて、複数の第１特徴点ｃ１の第１領域ｒ１内における第１密度ｄ１が、複数の第２特徴点ｃ２の第２領域ｒ２における第２密度ｄ２よりも高くなるように検出する検出処理を実施する。
【００３７】
上述の図５（ａ）の例において、並進方向が移動体４０の前後軸５１の方向、つまり、前後方向である場合、移動体４０が直進していると推定される。この場合、第２領域ｒ２は、第１画像１１の中心部（例えば、ｐ２、ｐ５、ｐ８）を含む。第１画像１１内において、第１領域ｒ１は、第２領域ｒ２の周り（例えば、ｐ１、ｐ３、ｐ４、ｐ６、ｐ７、ｐ９）に設けられる。つまり、第１画像１１の左側部分（ｐ１、ｐ４、ｐ７）と右側部分（ｐ３、ｐ６、ｐ９）とが第１領域ｒ１となる。第１画像１１の中央部分（ｐ２、ｐ５、ｐ８）が第２領域ｒ２となる。複数の第１特徴点ｃ１の第１領域ｒ１内における第１密度ｄ１が、複数の第２特徴点ｃ２の第２領域ｒ２内における第２密度ｄ２よりも高くなるように検出される。
【００３８】
図７は、特徴点検出部２３での特徴点検出処理を例示するフローチャート図である。  
特徴点検出部２３は、方向推定部２２で推定された回転方向（または並進方向）の情報に基づいて、移動体４０が停止しているか否かを判定し（ステップＳ１１）、停止していると判定した場合（ＹＥＳの場合）、終了する。ステップＳ１１において、停止していないと判定した場合（ＮＯの場合）、移動体４０が前後移動を行っているか否かを判定する（ステップＳ１２）。ここで前後移動とは、移動体４０が前後軸５１の方向に直進している、または、移動体４０が前後軸５１回りに回転していることを意味する。
【００３９】
特徴点検出部２３は、ステップＳ１２において、前後移動を行っていると判定した場合（ＹＥＳの場合）、第１画像１１の左右両側の画像領域（第１領域ｒ１）に含まれる第１特徴点ｃ１の第１密度ｄ１を相対的に増加させる（ステップＳ１３）。一方、ステップＳ１２において、前後移動を行っていないと判定した場合（ＮＯの場合）、移動体４０が右回転を行っているか否かを判定する（ステップＳ１４）。
【００４０】
特徴点検出部２３は、ステップＳ１４において、右回転を行っていると判定した場合（ＹＥＳの場合）、第１画像１１の左側の画像領域（第１領域ｒ１）に含まれる第１特徴点ｃ１の第１密度ｄ１を相対的に増加させる（ステップＳ１５）。一方、ステップＳ１４において、右回転を行っていないと判定した場合（ＮＯの場合）、移動体４０が左回転を行っているか否かを判定する（ステップＳ１６）。
【００４１】
特徴点検出部２３は、ステップＳ１６において、左回転を行っていると判定した場合（ＹＥＳの場合）、第１画像１１の右側の画像領域（第１領域ｒ１）に含まれる第１特徴点ｃ１の第１密度ｄ１を相対的に増加させる（ステップＳ１７）。一方、ステップＳ１６において、左回転を行っていないと判定した場合（ＮＯの場合）、特徴点数の増減を行わずに終了する。
【００４２】
図８は、任意回転での特徴点検出部２３の処理を例示する模式図である。  
上述の例では撮像部３０が平面移動すると仮定したが、撮像部３０が実空間を任意に動く場合についても同様に適用できる。例えば、図８に表すように、撮像部３０が、Ｘ軸とＹ軸との間で合成されたａ軸について回転している場合、回転後にＺ軸について正となる画像部分ｐ１０における第１特徴点ｃ１の第１密度ｄ１を相対的に高くすることで調整する。
【００４３】
さらに、特徴点検出部２３は、ステップＳ４で検出した第１特徴点ｃ１及び第２特徴点ｃ２の数が閾値以上であるか否かを判定する（ステップＳ５）。閾値未満であると判定した場合（ＮＯの場合）、ステップＳ１に戻り処理を繰り返す。ステップＳ５において、閾値以上と判定した場合（ＹＥＳの場合）、位置推定部２４が位置推定処理を実施する（ステップＳ６）。
【００４４】
図９（ａ）及び図９（ｂ）は、位置推定部２４での処理を例示する模式図である。  
図９（ａ）は、第１画像１１を例示する図である。  
図９（ｂ）は、第１画像１１に対応する第２画像１２を例示する図である。  
【００４５】
図９（ａ）及び図９（ｂ）において、位置推定部２４は、第２画像１２の第３領域ｒ３内の複数の第１対応点ｃ３を求める。複数の第１対応点ｃ３のそれぞれは、複数の第１特徴点ｃ１の少なくともいずれかのそれぞれに対応する。位置推定部２４は、複数の第１特徴点ｃ１のそれぞれと、複数の第１対応点ｃ３のそれぞれと、に基づいて、移動体４０の位置の変化を推定する位置推定処理を実施する。
【００４６】
なお、第２画像１２は、第２領域ｒ２に対応する第４領域ｒ４を含んでいてもよい。この場合、位置推定部２４は、第４領域ｒ４内の複数の第２対応点ｃ４を求める。複数の第２対応点ｃ４のそれぞれは、複数の第２特徴点ｃ２の少なくともいずれかのそれぞれに対応する。位置推定部２４は、複数の第２特徴点ｃ２のそれぞれと、複数の第２対応点ｃ４のそれぞれと、に基づいて、移動体４０の位置の変化を推定する。なお、この例では、第２特徴点ｃ２と第２対応点ｃ４とはそれぞれ１つとしている。
【００４７】
一般的に対応点の数が多いほど位置推定の精度が向上する。このため、第１領域ｒ１と第３領域ｒ３との間、及び、第２領域ｒ２と第４領域ｒ４との間で対応点探索をすることがより好ましい。
【００４８】
図１０は、位置推定部２４での位置推定処理を例示するフローチャート図である。  
位置推定部２４は、第１画像１１上から検出された第１特徴点ｃ１及び第２特徴点ｃ２に基づいて、例えば、パターンマッチング法や、特徴量マッチング法などを用いて、第２画像１２の第１対応点ｃ３及び第２対応点ｃ４を探索する（ステップＳ２１）。
【００４９】
位置推定部２４は、５つ以上の特徴点−対応点のペアから、例えば、５点法や７点法などを用いて、第１画像１１（または第２画像１２）を基準とする基本行列を推定する（ステップＳ２２）。なお、基本行列とは、二つのカメラ間の相対的な位置・姿勢変化を表した行列である。推定された基本行列から特異値分解などにより、並進ベクトルと回転行列を計算する。これら並進ベクトルと回転行列は、それぞれ撮像部３０の位置と姿勢に対応する。したがって、第１画像１１の撮影時の撮像部３０の位置と姿勢を基準とする、第２画像１２の撮影時の撮像部３０の位置と姿勢を得る（ステップＳ２３）。または、第２画像１２の撮影時の撮像部３０の位置と姿勢を基準とする、第１画像１１の撮影時の撮像部３０の位置と姿勢を得る。ただし、基本行列から撮像部３０の相対的な位置と姿勢を推定する方法は一例であり、基礎行列を推定する方法なども利用できる。なお、基礎行列とは、カメラ内部パラメータ（焦点距離、光軸中心、歪み特性など）が未知の場合におけるカメラ間の相対的な位置・姿勢を表す行列である。
【００５０】
実施形態によれば、周辺環境の変化や移動体の運動に対して頑健（ロバスト）な特徴点検出が可能となり、位置推定の頑健な高精度化・高速化が行える。ここでいう頑健とは、例えば、高精度な位置推定を行えることを含む。位置推定の精度を維持しながら、処理速度を低下させないことを含む。
【００５１】
ここで、撮像部３０で撮像された画像から移動体４０の位置を推定するときに、特徴点の有用性が重要である。ここでいう有用性とは、特徴点の追跡容易性、３次元実空間上における静止性、画像上における移動性、空間的な不偏性などである。このような有用な特徴点を利用する方法は多く提案されている。例えば、第１の参考例として、画像領域を経験的に設定し、画像領域ごとにオプティカルフローの長さに対する閾値を経験的に設定する方法がある。第２の参考例として、超音波センサにより障害物領域をおおまかに推定し、障害物領域に応じて画像領域ごとのエッジ強度や明度差に対する閾値を調整する方法がある。第３の参考例として、実空間上における移動体と特徴点間の相対的な位置および方向を用いて実空間上で特徴点位置の偏りが小さくなるように特徴点を選択する方法がある。
【００５２】
第１の参考例では、閾値が固定されているため、周辺環境の変化や移動体の移動に対して位置の推定精度が低下する。第２の参考例では、動的に閾値を調整するが、超音波センサでは遠方に対応できない。第３の参考例では、実空間上における特徴点の位置を事前に計測する必要がある。
【００５３】
これに対して、実施形態によれば、実空間上における特徴点の位置を必要とせず、周辺環境の変化や移動体の移動に対して頑健（ロバスト）な特徴点を検出することができる。
【００５４】
すなわち、実施形態では、第１画像１１及び第２画像１２に基づいて、移動体４０の移動の回転方向（または並進方向）を推定し、推定した回転方向（または並進方向）に基づいて、第１画像１１内の特徴点の密度を領域毎に動的に調整する。第１領域ｒ１は、移動体４０の移動の回転方向（または並進方向）に基づいて決定される。第１領域ｒ１は、移動体４０の周辺領域のうちで、移動体４０の移動に最も密接に関連する領域と言える。このため、第１領域ｒ１での複数の第１特徴点ｃ１の第１密度ｄ１を、第２領域ｒ２での複数の第２特徴点ｃ２の第２密度ｄ２よりも相対的に高くすることで、位置推定の精度が向上する。
【００５５】
また、第２領域ｒ２は、第１領域ｒ１と比べて、移動体４０との関連が密接ではない。このため、第２領域ｒ２の第２特徴点ｃ２を多少間引きしても大きな影響はないと考えられる。この間引きにより、処理速度を向上させることができる。
【００５６】
（第２の実施形態）
図１１は、第２の実施形態に係る移動体位置推定装置を例示するブロック図である。  
第１の実施形態では、２つの画像を用いて、回転方向（または並進方向）を推定した。第２の実施形態では、センサを用いて、回転方向（または並進方向）を推定する。
【００５７】
センサ６０は移動体４０に搭載される。センサ６０は撮像部３０に搭載される。センサ６０としては、速度センサ６１と、加速度センサ６２と、角速度センサ６３と、角加速度センサ６４と、が用いられる。取得部１０は情報１３を取得する。情報１３は、角速度センサ６３より得られた角速度に関する情報６３ａを含む。処理部２０は、角速度に関する情報６３ａに基づいて、回転方向を推定する。情報１３は、角加速度センサ６４より得られた角加速度に関する情報６４ａを含む。処理部２０は、角加速度に関する情報６４ａに基づいて、回転方向を推定する。
【００５８】
情報１３は、速度センサ６１より得られた速度に関する情報６１ａを含む。処理部２０は、速度に関する情報６１ａに基づいて、並進方向を推定する。情報１３は、加速度センサ６２より得られた加速度に関する情報６２ａを含む。処理部２０は、加速度に関する情報６２ａに基づいて、並進方向を推定する。
【００５９】
速度センサ６１または加速度センサ６２は、一軸以上とされる。角速度センサ６３または角加速度センサ６４についても、一軸以上とされる。速度センサ６１の出力をｖｏ、加速度センサ６２の出力をａｏとすると、撮像部３０の移動速度Δｔは、
Δｔ＝ｖｏまたはΔｔ＝ａｏ×τ
となる。τは時間を意味する。Δｔの成分のうち絶対値が最大となる方向を並進方向とする。ただし、並進方向の決定方法は一例である。
【００６０】
センサが三軸未満の場合、移動体主進行方向であるＺ成分を優先して計測する。角速度センサ６３の出力をωｏ、角加速度センサ６４の出力をαｏとすると、撮像部３０の回転ΔＲは、
ΔＲ＝ωｏまたはΔＲ＝αｏ×τ
となる。ΔＲの成分のうち絶対値が閾値以上となる方向を回転方向とする。ただし、回転方向の決定方法は一例である。
【００６１】
上述したように、実施形態によれば、撮像部３０の回転方向（または並進方向）をリアルタイムに取得できるため、画像による位置及び姿勢の推定が困難な急激な移動体の運動に対して頑健な特徴点検出が可能となる。移動体４０の位置推定の高精度化・高速化が行える。
【００６２】
この例では、撮像部３０の奥行き方向が移動体主進行方向となるように設置した場合について説明した。撮像部３０の設置が変更された場合は、撮像部３０の設置位置、姿勢に合わせて領域分割方法や特徴点検出方法を変更して用いることができる。
【００６３】
以上、実施形態として移動体位置推定装置、移動体位置推定方法を例示して説明したが、実施形態は、この移動体位置推定方法をコンピュータに実行させるための移動体位置推定プログラムの形態、あるいは、この移動体位置推定プログラムを記録したコンピュータ読み取り可能な記録媒体の形態としてもよい。
【００６４】
上記の記録媒体としては、具体的には、ＣＤ-ＲＯＭ（-Ｒ／-ＲＷ）、光磁気ディスク、ＨＤ（ハードディスク）、ＤＶＤ-ＲＯＭ（-Ｒ／-ＲＷ／-ＲＡＭ）、ＦＤ（フレキシブルディスク）、フラッシュメモリ、メモリカードや、メモリスティック及びその他各種ＲＯＭやＲＡＭ等が想定でき、これら記録媒体に上述した本実施形態の移動体位置推定方法をコンピュータに実行させるための移動体位置推定プログラムを記録して流通させることにより、当該方法の実現を容易にする。そしてコンピュータ等の情報処理装置に上記のごとくの記録媒体を装着して情報処理装置により移動体位置推定プログラムを読み出すか、若しくは情報処理装置が備えている記憶媒体に当該移動体位置推定プログラムを記憶させておき、必要に応じて読み出すことにより、本実施形態の移動体位置推定方法を実行することができる。
【００６５】
実施形態によれば、高精度な位置推定を行える移動体位置推定装置、移動体位置推定方法及び移動体位置推定プログラムが提供できる。
【００６６】
以上、具体例を参照しつつ、本発明の実施の形態について説明した。しかし、本発明は、これらの具体例に限定されるものではない。例えば、取得部及び処理部などの各要素の具体的な構成に関しては、当業者が公知の範囲から適宜選択することにより本発明を同様に実施し、同様の効果を得ることができる限り、本発明の範囲に包含される。
【００６７】
また、各具体例のいずれか２つ以上の要素を技術的に可能な範囲で組み合わせたものも、本発明の要旨を包含する限り本発明の範囲に含まれる。
【００６８】
その他、本発明の実施の形態として上述した移動体位置推定装置、移動体位置推定方法及び移動体位置推定プログラムを基にして、当業者が適宜設計変更して実施し得る全ての移動体位置推定装置、移動体位置推定方法及び移動体位置推定プログラムも、本発明の要旨を包含する限り、本発明の範囲に属する。
【００６９】
その他、本発明の思想の範疇において、当業者であれば、各種の変更例及び修正例に想到し得るものであり、それら変更例及び修正例についても本発明の範囲に属するものと了解される。
【００７０】
本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
【符号の説明】
【００７１】
１０…取得部、  １１…第１画像、  １１ａ…第１データ、  １２…第２画像、  １２ａ…第２データ、  １３…情報、  ２０…処理部、  ２１…画像分割部、  ２２…方向推定部、  ２３…特徴点検出部、  ２４…位置推定部、  ３０…撮像部、  ４０…移動体、  ４１…画像、  ４０ａ…第１方向、  ４０ｂ…第２方向、  ５１…前後軸、  ５２…上下軸、  ５３…左右軸、  ６０…センサ、  ６１…速度センサ、  ６１ａ…速度に関する情報、  ６２…加速度センサ、  ６２ａ…加速度に関する情報、  ６３…角速度センサ、  ６３ａ…角速度に関する情報、  ６４…角加速度センサ、  ６４ａ…角加速度に関する情報、  ｃ１、ｃ２…第１、第２特徴点、  ｃ３、ｃ４…第１、第２対応点、  ｄ１、ｄ２…第１、第２密度、  ｒ１〜ｒ４…第１〜第４領域
【図１】
2016061604.tif 000003
【図２】
2016061604.tif 000004
【図３】
2016061604.tif 000005
【図６】
2016061604.tif 000006
【図７】
2016061604.tif 000007
【図１０】
2016061604.tif 000008
【図１１】
2016061604.tif 000009
【図４】
2016061604.tif 000010
【図５】
2016061604.tif 000011
【図８】
2016061604.tif 000012
【図９】
2016061604.tif 000013