JP2017134617A - 位置推定装置、プログラム、位置推定方法 - Google Patents
位置推定装置、プログラム、位置推定方法 Download PDFInfo
- Publication number
- JP2017134617A JP2017134617A JP2016013860A JP2016013860A JP2017134617A JP 2017134617 A JP2017134617 A JP 2017134617A JP 2016013860 A JP2016013860 A JP 2016013860A JP 2016013860 A JP2016013860 A JP 2016013860A JP 2017134617 A JP2017134617 A JP 2017134617A
- Authority
- JP
- Japan
- Prior art keywords
- image
- feature point
- motion
- unit
- moving body
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims description 71
- 230000033001 locomotion Effects 0.000 claims abstract description 172
- 238000003384 imaging method Methods 0.000 claims abstract description 51
- 238000000605 extraction Methods 0.000 claims abstract description 20
- 238000001514 detection method Methods 0.000 claims abstract description 7
- 238000003860 storage Methods 0.000 claims description 30
- 239000000284 extract Substances 0.000 claims description 9
- 230000010365 information processing Effects 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 abstract description 3
- 238000005259 measurement Methods 0.000 description 60
- 230000008569 process Effects 0.000 description 43
- 239000011159 matrix material Substances 0.000 description 37
- 238000012545 processing Methods 0.000 description 20
- 238000013519 translation Methods 0.000 description 20
- 230000008859 change Effects 0.000 description 14
- 238000004364 calculation method Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 5
- 238000012937 correction Methods 0.000 description 5
- 230000014509 gene expression Effects 0.000 description 5
- 230000007613 environmental effect Effects 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 230000001174 ascending effect Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000004913 activation Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000012217 deletion Methods 0.000 description 1
- 230000037430 deletion Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000446 fuel Substances 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000003909 pattern recognition Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 229920006395 saturated elastomer Polymers 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Image Analysis (AREA)
Abstract
【課題】推定される位置の精度を向上できる位置推定装置を提供すること。【解決手段】撮像装置が撮像した画像から特徴点を抽出する画像特徴点抽出部11と、時間的に前に撮像された画像の特徴点を追跡する画像特徴点追跡部12と、移動体の三次元座標、距離情報及び撮像装置のカメラパラメータを用いて、特徴点の三次元座標を含む地図を生成する環境地図生成部15と、三次元座標を画像に投影した場合の投影座標と特徴点の座標の誤差、又は、特徴点の座標を三次元座標に変換した場合の変換座標と三次元座標の誤差がより小さくなる移動体の位置と姿勢を検出する位置検出部16と、位置検出部が位置に基づき検出する移動体の移動速度、姿勢に基づき検出する移動体の回転速度、及び、画像の撮像条件に基づいて、特徴点を追跡可能な画像を撮像できる運動を定義する許容運動定義部17と、を有する。【選択図】図6
Description
本発明は、位置推定装置、プログラム及び位置推定方法に関する。
運転者が移動体を操縦したり外部から操縦しなくても移動体が自立的に決められた経路に沿って移動したりする技術の開発が進められている。移動体が経路に沿って移動するには移動中も移動体が位置を正確に推定する必要がある。そして、移動体の位置や姿勢を移動体が備えるカメラを用いて推定するVisual−SLAM(Simultaneous Localization and Mapping;同時位置推定地図構築)が知られている。Visual−SLAMでは、時系列の映像から周囲の静止物体の三次元位置情報を含む環境地図の生成と、移動体の位置や姿勢の推定とを同時に行うことができる。
カメラを用いた位置と姿勢の検出では、時系列に撮像された画像から複数の特徴点とその三次元座標を求めておき、次のタイミングで撮像された画像の特徴点との対応付けを行う。次のタイミングの画像のうち追跡された特徴点の三次元座標が分かっているので、該三次元座標と画像内の特徴点との対応を利用して自己位置と姿勢の推定が行われる。
このようにカメラを用いて位置と姿勢が検出される場合、位置と姿勢の精度が撮像された映像の品質に大きく依存することが知られている。そこで、カメラによる映像の品質に対しロバストな位置推定技術が従来から考案されている(例えば、特許文献1参照。)。特許文献1には、異なるタイミングで撮像された複数の画像のそれぞれから対応した対応点を検索し、検索された対応点の全ての組から対応点の複数の組を繰り返し選択し、繰り返し選択された対応点の複数の組に基づいて自車両の運動推定候補を各々算出する運動推定装置が開示されている。
しかしながら、従来の技術では、特徴点の対応付けが困難なほど画像が大きく変化した場合に、推定される位置や姿勢の精度が低下するおそれがあるという問題がある。例えば、移動体は目的地に早く到着することが望ましいと考えられるが、移動体が早く移動するために位置や姿勢の変化が大きくなると異なるタイミングで撮像された画像内の特徴点の対応付けが困難になる場合がある。この場合、過去に対応付けが可能であった画像を用いて位置や姿勢が推定されるので、推定される位置や姿勢の精度が低下するおそれがある。
上術した課題に鑑み、本発明は、推定される位置の精度を向上できる位置推定装置を提供することを目的とする。
本発明は、被写体までの距離情報を検出可能な撮像装置が撮像した周囲の画像から移動体の位置を推定すると共に周囲の地図を作成する位置推定装置であって、前記撮像装置が撮像した前記画像から特徴点を抽出する画像特徴点抽出部と、前記画像の前記特徴点を前記撮像装置が撮像した前記画像において追跡する画像特徴点追跡部と、前記移動体の三次元座標、前記距離情報及び前記撮像装置のカメラパラメータを用いて、前記特徴点の三次元座標を含む前記地図を生成する環境地図生成部と、前記三次元座標を前記画像に投影した場合の投影座標と前記特徴点の座標の誤差、又は、前記特徴点の座標を前記三次元座標に変換した場合の変換座標と前記三次元座標の誤差がより小さくなる前記移動体の位置と姿勢を検出する位置検出部と、前記位置検出部が前記位置に基づき検出する前記移動体の移動速度又は前記姿勢に基づき検出する前記移動体の回転速度の少なくともいずれかと、前記画像の撮像条件に基づいて、前記特徴点を追跡可能な前記画像を撮像できる運動を定義する許容運動定義部と、を有する。
推定される位置の精度を向上できる位置推定装置を提供することができる。
以下、本発明を実施するための形態について図面を参照しながら説明する。
図1は、移動体の概略斜視図の一例である。図1の移動体はマルチコプターやドローンなどと呼ばれる飛行物体100である。この飛行物体100は放射状に配置される複数のロータ(回転翼)9を回転させることによって上昇・下降し、また、前後左右等に移動する。上昇時には4つのロータ9の回転数を増大させ、下降時には4つのロータ9の回転数を減少させる。また、前進時又は後退時には対称にあるロータ9の回転数に変化を付け機体を傾けることで前進又は後退する。
図1は、移動体の概略斜視図の一例である。図1の移動体はマルチコプターやドローンなどと呼ばれる飛行物体100である。この飛行物体100は放射状に配置される複数のロータ(回転翼)9を回転させることによって上昇・下降し、また、前後左右等に移動する。上昇時には4つのロータ9の回転数を増大させ、下降時には4つのロータ9の回転数を減少させる。また、前進時又は後退時には対称にあるロータ9の回転数に変化を付け機体を傾けることで前進又は後退する。
本実施形態の飛行物体100は、Visual−SLAM(Simultaneous Localization and Mapping;同時位置推定地図構築)を使用して、時系列の映像から周囲の静止物体(三次元点)の三次元座標を含む環境地図の生成と、自己位置や姿勢の推定とを同時に行う。
図1の飛行物体100は、位置を計測する位置計測部10及び移動に関する制御を行う移動体制御部30を有している。位置計測部10は飛行物体100の現在の位置と姿勢を推定、計測又は算出等するものであり、移動体制御部30は現在の位置を参考にしながら目的地まで飛行物体100を移動させる。また、飛行物体100はステレオカメラ50を有している。ステレオカメラ50は、被写体との距離を検出できるという特徴を有する。距離を検出可能な手段を別に設ければ単眼のカメラでもよいし、距離情報を取得できる単眼カメラでもよい。距離を検出可能な手段の一例としてレーザーレーダが挙げられる。
図2は、位置計測部10と移動体制御部30のハードウェア構成図の一例である。位置計測部10は、例えば、スマートフォン、タブレット端末、ノートPC、デジタルカメラ、又は、飛行物体100や車両、移動式ロボット等の各種移動体に設けられた制御装置を本体とすることができる。したがって、位置計測部10は情報処理装置としての機能を備える。
位置計測部10は、入力装置101と、出力装置102と、ドライブ装置103と、カメラI/F104と、位置計測装置105と、移動体制御部I/F106、補助記憶装置107と、主記憶装置108と、各種制御を行うCPU(Central Processing Unit)109と、ネットワーク接続装置110と、姿勢センサ111と、論理回路112とを有し、これらはシステムバスBで相互に接続されている。
入力装置101は、ユーザ等が操作するキーボード、マウス等のポインティングデバイスやマイク等の音声入力デバイス等を有し、ユーザ等からの各種操作信号の入力を受け付ける。また、入力装置101として出力装置102と一体のタッチパネルを備えていてもよい。
出力装置102は、位置計測部10の状態を示すLED、CUI(Character User Interface)やGUI(Graphical User Interface)などで各種の情報を表示するディスプレイなどであり、位置計測部10の処理結果や状態を外部に出力する。更に、音声を出力するスピーカの機能を備えていてよい。
ドライブ装置103には、例えば記録媒体103a等が着脱自在に装着され、CPU109からの制御で記録媒体103aに記録された各種情報を読み込んだり、各種情報を記録媒体103aに書き込んだりすることができる。位置計測部10が実行するプログラムは、例えば記録媒体103a等に記憶された状態で配布されたり、プログラムを配信したりするサーバから配布される。
記録媒体103aは、各種のプログラムやデータ等を記憶するコンピュータで読み取り可能な媒体である。記録媒体103aは、例えばUSB(Universal Serial Bus)メモリやSDメモリ、CD−ROM等の可搬型である。
カメラI/F104はステレオカメラ50とのインタフェースである。カメラI/F104はステレオカメラ50にカメラパラメータ(主に内部パラメータをいい、例えば焦点距離、露光時間、画像中心、絞り値、ISO感度、アスペクト比、スキュー補正量、ゆがみ補正量、フレーム間隔など)を送信し、ステレオカメラ50から画像データやカメラパラメータを受信したりする。
位置計測装置105は、GPS(Global Positioning System;全地球測位システム)機能等を用いて、飛行物体100の位置情報(座標:緯度、経度、標高)を取得する。室内や地下ではIMES(Indoor Messaging System)やビーコンから位置情報を取得してもよい。
移動体制御部I/F106は、移動体制御部30と通信するためのインタフェースである。具体的にはUSBケーブル、I2C,SPI、MicroWireなどが接続される。
補助記憶装置107は、例えばSSD(Solid State Drive)やHDD(Hard Disk Drive)等のストレージ装置等である。補助記憶装置107は、CPU109が図2の各ブロックへ出力する制御信号を生成するためのプログラム、オペレーティングシステム等が記憶されている。
主記憶装置108には、補助記憶装置107から読み出されたプログラム等を展開され、CPU109により実行される。主記憶装置108は、例えばRAM(Random Access Memory)等であるが、これに限定されるものではない。
CPU109は、オペレーティングシステムやプログラムに基づいて、各種演算や図2の各部に対し制御信号を出力し、位置計測部10の全体の処理を制御する。プログラムの実行結果等を補助記憶装置107に格納することができる。
ネットワーク接続装置110は、CPU109からの制御信号に基づき、各種のネットワークに接続する通信装置である。例えば、無線LAN(Local Area Network)や3G、4Gの公共無線通信網に接続する。あるいは有線LANに接続してもよい。これにより、ネットワーク接続装置110は、プログラムや設定情報等をネットワークに接続されている外部装置等から取得できる。また、ネットワーク接続装置110は、プログラムを実行することで得られた実行結果等を外部装置等に提供することができる。
姿勢センサ111は、飛行物体100の姿勢を検出するための各種のセンサである。例えば、3軸のジャイロセンサ、3軸の加速度センサ、及び、磁気方位センサを有する。
論理回路112は、特定の情報処理を行うハードウェアである。具体的には、AND、OR、NAND、NOR回路などを要素として、加算器、乗算器、メモリ、ラッチなど、及び、これらが組み合わされたIC、LSI、ASIC、FPGAなどである。本実施形態では、後述する特徴点の抽出、追跡、及び、視差演算などを論理回路112が行う。
本実施形態では、上述したコンピュータ本体のハードウェア構成に実行プログラム(例えば、位置姿勢計測プログラム等)をインストールすることで、ハードウェア資源とソフトウェアとが協働して本実施形態における位置姿勢計測処理等を実現することができる。また、上述した位置姿勢計測処理に対応するプログラムは、例えば装置上で常駐している状態であってもよく、起動指示により起動させてもよい。
続いて、移動体制御部30について説明する。移動体制御部30は飛行物体100の運動を制御する。運動とは、位置、移動速度、及び、姿勢の少なくとも1つをいう。移動体制御部30は、位置計測部I/F201、演算制御装置202、モータコントローラ203及び4つのモータ204を有している。
位置計測部I/F201は、位置計測部10と通信するためのインタフェースである。具体的にはUSBケーブル、I2C,SPI、MicroWireなどが接続される。例えば、位置計測部10が推定した位置、移動速度及び姿勢、並びに後述する許容運動マップが位置計測部I/F201を介して送信される。
演算制御装置202は、4つのモータ204の回転数を制御することでロータ9の回転数を制御し、位置(三次元空間の位置)、移動速度、姿勢などを制御する。4個のロータ9の1つ以上を揚力発生用のロータ9や姿勢制御用のロータ9として動作するように制御する。更に、移動の際には、左右又は前後のロータ9の回転数をアンバランス(回転数差を設けて)にして機体を傾けるように制御することにより移動させる。なお、ロータ9の回転により生じる飛行物体100への回転トルクを抑制するため、4個のロータ9のうち、2つを時計回り方向へ回転させ、残りの2つを反時計回り方向へ回転させる。
演算制御装置202には予め経由地を含む目的地の座標が登録されており、位置計測部10が検出する位置と姿勢に基づいて、経由地又は目的地に接近するように飛行物体100の移動方向を決定する。適切な移動速度は予め定められているものとする。演算制御装置202は移動方向と移動速度等により決定した4つのロータ9の回転数を指示する回転数指示信号をモータコントローラ203に出力する。
モータコントローラ203は回転数指示信号に応じてモータ204の回転数を制御する。回転数指示信号に応じた電流がモータ204に流れるように例えばPWM信号を生成しモータ204に出力する。モータコントローラ203はモータ204の回転数を検出又は取得してフィードバック制御を行う。また、同一回転数で回転すべきモータ204に微小な回転数の違いがあれば、同一回転数となるように電流を制御する。更に、2つのロータ9の間で回転数指示信号に応じた回転数差が得られない場合、指示された回転数差が得られるように少なくとも1つのモータ204に流れる電流を制御する。
<位置計測部10の機能構成>
続いて、図3を用いて位置計測部10の機能について説明する。図3は、位置計測部10の機能ブロック図の一例である。位置計測部10は、画像特徴点抽出部11、画像特徴点追跡部12、深度計測部13、撮像部制御部14、環境地図生成部15、移動***置計測部16、及び、許容運動定義部17を有する。
続いて、図3を用いて位置計測部10の機能について説明する。図3は、位置計測部10の機能ブロック図の一例である。位置計測部10は、画像特徴点抽出部11、画像特徴点追跡部12、深度計測部13、撮像部制御部14、環境地図生成部15、移動***置計測部16、及び、許容運動定義部17を有する。
画像特徴点抽出部11、画像特徴点追跡部12及び深度計測部13は上述の論理回路112により実現される。撮像部制御部14、環境地図生成部15、移動***置計測部16、及び、許容運動定義部17は、補助記憶装置107から主記憶装置108に展開されたプログラムをCPU109が実行することで実現される機能又は手段である。ただし、画像特徴点抽出部11、画像特徴点追跡部12及び深度計測部13が、CPU109がプログラムを実行することで実現されてもよい。
また、位置計測部10は記憶部1000を有している。記憶部1000は、補助記憶装置107又は主記憶装置108の少なくとも一方により構築される情報の記憶手段である。
撮像部制御部14は、カメラパラメータをステレオカメラ50に出力して、ステレオカメラ50を制御する。本実施形態では、カメラパラメータのうち、少なくとも露光時間とフレーム間隔が出力される。フレーム間隔(撮像間隔)は固定でもよいし撮像される環境などに応じて撮像部制御部14が制御してもよい。固定の場合、フレーム間隔は飛行物体100の補助記憶装置107などに記録されている。また、撮像部制御部14は、ステレオカメラ50が撮像する画像のヒストグラムを利用して、例えば飽和する画素がなく低輝度から高輝度までの使用されるように露光時間を決定する。
深度計測部13は、ステレオ計測法により各画素又は画素ブロックごとにステレオカメラ50から被写体までの距離を計測する。いわゆる、ブロックマッチング法により画像を一定の大きさのブロックに分割し、それぞれのブロックの水平方向のずれを視差として算出する。更に、ブロックコストを算出した後、周囲との滑らかさを考慮して視差を最適化するSemi-Global-Mating法などを適用することが好ましい。詳細は例えば非特許文献1に開示されている。
画像特徴点抽出部11は、ステレオカメラ50が撮像した画像から特徴点(画像特徴点)を抽出する。画像特徴点抽出部11は、撮像した画像毎に特徴点を抽出する。画像特徴点抽出部11は、例えば画像の輝度値を用いて特徴点を抽出したり、画像中の物体の形状等から特徴点を抽出したりする手法があるが、これらに限定されるものではない。
特徴点の抽出手法としては、例えばHarrisオペレータやFAST(Features from Accelerated Segment Test)オペレータを用いることができる。Harrisオペレータは、2つのエッジの交点をコーナーとして検出する画像処理である。FASTオペレータは、注目画素の周囲の円周上における16個の画素の画素値が連続してn個以上、注目画素より明るいか暗い場合にコーナーを検出する画像処理である。その他、特徴点の抽出手法として、例えばSIFT(Scale Invariant Feature Transform)特徴量記述子、SURF(Speeded Up Robust Features)特徴量記述子を用いることができる。画像特徴点抽出部11は、画像上の特徴点の座標とその特徴量(画素値、濃度、輝度など)とを画像特徴点情報として記憶部1000に記録する。
画像特徴点追跡部12は、時系列の(異なるタイミングで撮像された)画像の特徴点を追跡する。画像特徴点追跡部12は、例えば画像特徴点抽出部11が抽出した特徴点を、その特徴点を抽出した画像より後にステレオカメラ50が撮像した画像の中から探索することで各特徴点の追跡を行う。また、画像特徴点追跡部12は、探索結果から追跡の成否(成功、失敗)を判定し、その判定結果に基づき記憶部1000の画像特徴点情報に登録等を行う。すなわち、画像の特徴点ごとに追跡の成否が登録される。
画像特徴点追跡部12は、例えば、ブロックマッチングや特徴量等による探索手法を用いて特徴点の追跡を行うことができる。ブロックマッチングを用いて特徴点の追跡を行う場合、画像特徴点追跡部12は、時系列の1つ前の画像の特徴点を含むブロックを取り出し、時系列の1つ後の画像に対しブロックマッチングを行う。
また、上述した特徴量を用いた探索を行う場合には、画像特徴点追跡部12は、画像特徴点抽出部11と同様の特徴点記述処理を行い、画像特徴点情報として記録されている特徴点と、相関の高い特徴点を探索する。また、画像特徴点追跡部12は、上述の探索の結果、相関が所定の閾値を超えている場合には追跡成功と判定し、閾値以下の場合には追跡失敗と判定する。
環境地図生成部15は、画像特徴点情報、カメラパラメータ、及び、深度計測部13の計測結果から、周囲の環境の三次元地図を生成する。例えば、飛行物体100の初期位置(後述するワールド座標系の三次元座標)は、位置計測装置105により既知である。この初期位置において、カメラパラメータの焦点距離と深度計測部13の結果により特徴点までの距離が分かるので、特徴点の三次元座標は図4のようにして求めることができる。
図4(a)は、ピンホールカメラモデルにおける画像内の特徴点の座標とワールド座標系の三次元座標の対応を説明する図の一例である。(x,y)は特徴点の画像上の座標で、(X,Y,Z)は特徴点のワールド座標系の三次元座標である。fは焦点距離である。深度計測部13により計測された深度(距離)がZであるので、図4(b)に示すxとX、fとZの比例関係からX=Z・(x-cx)/fが得られる。同様に、Y=Z・(y-cy)/fが得られる。cxとcyは画像中である(画像内の特徴点の座標が左上コーナーを原点としているため)。
したがって、飛行物体100の初期位置とそこで撮像された画像に含まれる三次元点(ワールド座標系における特徴点)の三次元座標が分かることになる。環境地図生成部15はこれにより簡易的な環境地図を生成しておき、後に最適化を行う。
図3に戻り、移動***置計測部16は、環境地図と画像特徴点情報から、自身の位置と姿勢を計測する処理を行う。初期位置から飛行物体100が移動して周囲を撮像した場合、異なる場所から同じ特徴点が撮像される。移動***置計測部16は、特徴点のワールド座標系における三次元座標をカメラ座標系に変換する回転行列R,平行移動ベクトルtを後述するように求めることで、移動後の位置と姿勢を計測する。そして、この移動後の位置と姿勢において図4のようにして特徴点の三次元座標が分かる。したがって、ワールド座標系において撮像時の飛行物体100の位置と姿勢を常に更新し、かつ、環境地図を生成しながら移動できる。
許容運動定義部17は、露光時間とフレーム間隔を元に、飛行物体100に許容される又は制限される運動を定義し移動体制御部30に出力する。移動体制御部30は制約の範囲内で飛行物体100の運動を制御する。詳細は後述される。
<<記憶部に記憶される情報>>
図5は記憶部1000に記憶される情報を模式的に示す図の一例である。記憶部1000には、環境地図1001、画像特徴点情報1002、カメラパラメータ1003、許容運動情報1004、移動***置姿勢情報1005、及び、運動パラメータ使用運動1006が記憶される。
図5は記憶部1000に記憶される情報を模式的に示す図の一例である。記憶部1000には、環境地図1001、画像特徴点情報1002、カメラパラメータ1003、許容運動情報1004、移動***置姿勢情報1005、及び、運動パラメータ使用運動1006が記憶される。
環境地図1001とは、特徴点の三次元座標が登録された地図である。特徴点ごとに三次元座標が登録されており、飛行物体100が移動した空間の地図が再現される。これにより、建物や壁等の物体、道路等がどこにあるか等の周辺環境の情報が得られる。なお、環境地図は、周囲の静止物体等の2次元の情報であってもよい。
画像特徴点情報1002は、画像ごとに以下の情報を有する。
画像上の特徴点の座標
特徴点の特徴量(画素値、濃度、輝度など)
時系列の1つ前の画像からの追跡の成否
カメラパラメータ1003については上述のように主に内部パラメータであり、すくなくとも露光時間とフレーム間隔を含む。露光時間とはCCDやCMOSの撮像素子が被写体光に晒されている時間である。フレーム間隔とは、ステレオカメラ50が1つの画像を撮像してから次の画像を撮像するまでの時間的な間隔である。
画像上の特徴点の座標
特徴点の特徴量(画素値、濃度、輝度など)
時系列の1つ前の画像からの追跡の成否
カメラパラメータ1003については上述のように主に内部パラメータであり、すくなくとも露光時間とフレーム間隔を含む。露光時間とはCCDやCMOSの撮像素子が被写体光に晒されている時間である。フレーム間隔とは、ステレオカメラ50が1つの画像を撮像してから次の画像を撮像するまでの時間的な間隔である。
許容運動情報1004は、複数の運動パラメータのうち、許容運動定義部17により許容された運動パラメータ又は運動パラメータを特定するための情報である。運動パラメータは、飛行物体100の運動の内容を規定するパラメータである。例えば、剛体の自由度は6つ(ピッチ角θ、ロール角φ、ヨー角ψの回転、X,Y,Z方向の移動)であるが、飛行物体100が例えば一定方向へ移動する場合でも移動体制御部30は複数のモータ204を制御する。このため、飛行物体の運動を制御しやすいように、運動パラメータがいくつか設定されている。運動パラメータは例えば、上昇、下降、右旋回、左旋回、前進、後進、右移動、左移動などであるがこれらに限られない。
移動***置姿勢情報1005は、特徴点が検出された画像を撮像したステレオカメラ50の位置と姿勢に関する情報である。移動***置姿勢情報1005には撮像した画像ごとに位置と姿勢が登録されている。
運動パラメータ使用運動1006は各運動パラメータで使用する運動を示す。例えば、上昇であれば、回転は伴わないので、ピッチ角θ、ロール角φ、及び、ヨー角ψが変化する運動は行われない。また、移動を伴わない姿勢変化では、位置が変化しない(後述する平行移動ベクトルがゼロ)。X方向への移動であれば、移動の原理に基づく姿勢の変化とX座標が変化する。運動パラメータ使用運動1006には運動パラメータごとにピッチ角θ、ロール角φ、ヨー角ψ、X座標、Y座標、Z座標が変化するかどうかが対応付けて登録されている。
<遺影計測部の処理>
次に、図6、7を用いて、本実施形態における位置姿勢計測処理の一例について、フローチャートを用いて説明する。図6は、本実施形態における位置計測部10の全体的な処理手順を示すフローチャート図の一例である。
次に、図6、7を用いて、本実施形態における位置姿勢計測処理の一例について、フローチャートを用いて説明する。図6は、本実施形態における位置計測部10の全体的な処理手順を示すフローチャート図の一例である。
ステレオカメラ50は、撮像部制御部14から通知されたカメラパラメータ等により画像を取得する(S10)。ステレオカメラ50は一定の周期で撮像を行い、時系列に画像を生成する。例えば、飛行物体100が移動している場合、ステレオカメラ50は、移動中の時系列な画像を生成する。ステレオカメラ50は、撮像した画像を、画像特徴点抽出部11、画像特徴点追跡部12、及び深度計測部13に出力する。
次に、画像特徴点抽出部11は、ステップS10で得られた画像から特徴点を抽出する(S20)。特徴点は記憶部1000の画像特徴点情報1002に記憶される。
次に、画像特徴点追跡部12は、連続する画像間において、S20の処理で得られた特徴点を追跡する(S30)。追跡の成否は記憶部1000の画像特徴点情報1002に記憶される。
深度計測部13は画像の画素又は画素ブロック毎に深度(距離情報)を計測する(S40)。深度は画像の画素又は画素ブロックに埋め込まれるので画像と共に参照される。あるいは、記憶部1000の画像特徴点情報1002に特徴点と対応付けて記憶してもよい。
なお、環境地図生成部15は、図4に示したように、記憶部1000に記憶されている画像の特徴点(座標)と、飛行物体の位置と姿勢及び深度を用いて環境地図を随時作成しておく。
次に、移動***置計測部16は、飛行物体100(又は、ステレオカメラ50)の位置と姿勢の計測を行う(S50)。位置と姿勢は記憶部1000の移動***置姿勢情報1005に記憶される。
次に、環境地図生成部15は、画像特徴点情報1002、及び、S50の処理で得られる移動***置姿勢情報1005等を用いて環境地図生成処理を行う(S60)。なお、S60の処理では、環境地図の生成だけでなく、環境地図の更新(例えば、補正、削除、拡張)等を行うこともできる。
次に、許容運動定義部17はカメラパラメータ1003と移動***置計測部16が算出する移動速度と回転速度に基づいて、画像における特徴点の移動量を算出し、許容できる運動パラメータを決定する(S70)。
<処理の詳細>
以下では、図6のステップS50、S60、S70について詳細に説明する。
以下では、図6のステップS50、S60、S70について詳細に説明する。
<<S50 位置姿勢計測処理>>
上述した位置計測部10における位置姿勢計測処理(S50)について具体的に説明する。図7は、位置姿勢計測処理を説明するための図の一例である。移動***置計測部16は、環境地図の生成で得られた特徴点の三次元座標と画像における特徴点の座標の対応関係を用いて位置と姿勢を計測する。
上述した位置計測部10における位置姿勢計測処理(S50)について具体的に説明する。図7は、位置姿勢計測処理を説明するための図の一例である。移動***置計測部16は、環境地図の生成で得られた特徴点の三次元座標と画像における特徴点の座標の対応関係を用いて位置と姿勢を計測する。
飛行物体100(ステレオカメラ50)と共に移動する座標系をカメラ座標系と称し、扱う三次元空間の全体の座標系をワールド座標系と称す。カメラ座標系は例えばカメラの撮像素子の中央を原点とする座標系である。ワールド座標系はGPSで使用されるWGS84座標系、世界測地系又は日本測地系などである。カメラ座標系のZ方向が飛行物体100の正面、X方向が側面、Y方向が下面であるとする。Y軸周りの回転により生じる姿勢はヨー角(ψ)で指定され、Z軸周りの回転により生じる姿勢はロール角(ρ)で指定され、X軸周りの回転により生じる姿勢はピッチ角(θ)で指定される。
位置計測部10は、ワールド座標系からカメラ座標系へ変換する回転行列R、平行移動ベクトルtを、位置と姿勢の推定により求める。カメラ座標系とワールド座標系とを変換する行列のうち回転成分の行列を回転行列R、平行移動成分の行列を平行移動ベクトルtと称する。
ここで、R、tは、それぞれ以下の(1)式で表される。
回転行列R、平行移動ベクトルtを求めるために、画像内にある特徴点のワールド座標系での三次元座標Xi w、及び、ステレオカメラ50の透視投影行列を利用する。透視投影行列には以下のカメラパラメータ1003が使用される。
・X方向の焦点距離fx
・Y方向の焦点距離fy
・X方向の画像中心ox
・Y方向の画像中心oy
三次元座標Xi w、透視投影行列は、それぞれ以下に示す(2)式、(3)式で表される。
・X方向の焦点距離fx
・Y方向の焦点距離fy
・X方向の画像中心ox
・Y方向の画像中心oy
三次元座標Xi w、透視投影行列は、それぞれ以下に示す(2)式、(3)式で表される。
特徴点の三次元座標については、上述した環境地図1001に登録されている。また、透視投影行列Pで使用される焦点距離や画像中心などはカメラパラメータ1003に登録されている。カメラパラメータ1003については、例えばカメラキャリブレーションによって担当者等が予め求めおくことができる。カメラキャリブレーションについては、例えばZhangの手法(Z.Zhang,"A flexible new technique for camera calibration",IEEE Transactions on Pattern Analysis and Machine Intelligence,22(14):1330-1334,2000)等を用いることができるが、これに限定されるものではない。
また、本実施形態において、移動***置計測部16は、上述の処理以外に、ステレオカメラ50の歪み補正を行ってもよい。なお、歪みのパラメータについてもZhangの手法で得ることができる。また、歪み補正については、特徴点抽出を行う前に画像自体の歪み補正を行ってもよく、特徴点を抽出した後に、歪みのパラメータ等を用いて補正してもよい。移動***置計測部16は、上述したいずれかの処理を行った後に、位置と姿勢の推定を行うことで、より高精度にステレオカメラ50又は飛行物体100の位置と姿勢を計測することができる。
移動***置計測部16は、三次元座標Xi w、及び、透視投影行列Pを利用して、再投影誤差を最小化にするR,t及び三次元点の座標を求める。再投影誤差は、特徴点の三次元座標を画像上に投影した点の座標(再投影座標)と、実際にそれがステレオカメラ50で観測された座標との差分により定義される。ステレオカメラ50で観測された座標とは、画像特徴点情報1002に登録された特徴点の画像内の座標である。
再投影座標は、例えば「三次元点のカメラ座標系への変換」及び「仮想スクリーンへの投影」により求めることができる。
まず、三次元点のカメラ座標系への変換は、以下に示す(4)式で求めることができる。(4)式では、カメラ座標系での三次元点の座標をXi cとした。
次に、仮想スクリーンへ再投影された三次元点の座標を求める。ここで、再投影点の座標をxi r、yi rとする。xi rは右カメラの画像のx座標、yi rは右カメラの画像のy座標である。再投影点の座標xi r、yi rは以下により求めることができる。
次に、再投影誤差は、(5)式で求めた再投影点の座標、及び、実際にステレオカメラ50で観測された特徴点の座標の2乗誤差の総和として、以下に示す(6)式で定義される。
一般に、非線形最小化問題は算出コストが高く、また適切な初期値を設定しないと適切な解が求まらないという課題がある。非線形最小化問題を解くには、線形化する方法、Levenberg-Marquardt法など利用する方法が知られている。
また、ステレオカメラ50が用いられているため、左右の画像を使用して、以下のように再投影誤差を最小化するRとtを求めてもよい。
以上のようにして回転行列R、平行移動ベクトルtを求めることができれば、飛行物体100の位置と姿勢を求めることができる。回転行列Rはワールド座標系に対する回転行列なので、ステレオカメラ50と一体の飛行物体100がワールド座標系に対しどのくらい傾いているかを示す情報となる。
位置については、平行移動ベクトルtがワールド座標系からの平行移動量を表すので、画像内にある特徴点のワールド座標系での三次元座標Xi wに平行移動ベクトルtを適用することで、カメラと共に移動する飛行物体100のワールド座標系における位置を決定できる。
なお、本実施形態では、再投影誤差が最小になる回転行列R、平行移動ベクトルtを求めたが、オブジェクトスペースエラー(Object space error)が最小になる回転行列R、平行移動ベクトルtを求めてもよい。オブジェクトスペースエラーは三次元座標の誤差である。この場合、移動***置計測部16はオブジェクトスペースエラーが最小になるように回転行列R、平行移動ベクトルtを求める。すなわち、透視投影行列P、回転行列R及び平行移動ベクトルtを画像の特徴点の座標に適用し三次元座標(変換座標)を求め、環境地図の三次元座標との差が最小になるように回転行列R、平行移動ベクトルtを求める。この場合、線形に解が求まるため算出コストを低減できるという利点がある。
<<S60 環境地図生成処理>>
次に、図8を用いて、上述した環境地図生成部15における環境地図生成処理について、フローチャートを用いて説明する。図8は、環境地図生成処理の一例示すフローチャートである。
次に、図8を用いて、上述した環境地図生成部15における環境地図生成処理について、フローチャートを用いて説明する。図8は、環境地図生成処理の一例示すフローチャートである。
図8において、環境地図生成部15は、環境地図全体の再投影誤差の最小化処理を行う(S601)。この再投影誤差の最小化処理はバンドル調整ともいう。姿勢(Rj,tj)は1回の撮像で1つ得られる(jは画像の撮像順)。また、各撮像(画像)でn個の三次元点の座標Xiが得られている。したがって、ステップS601の処理では、例えば、過去の撮像で得られたNj個の位置と姿勢(Rj,tj)と各撮像で得られたn個の三次元点の座標Xiをパラメータとして、再投影誤差が最小となる位置と姿勢を求める処理である。この処理は(13)式により表すことができる。
(13)式は、Nj個の撮像と各撮像で得られたn個の三次元点の座標Xiから再投影誤差が最も小さくなるように位置と姿勢(Rj,tj)と座標Xiを求めるので、(13)式を最小化する姿勢(Rj,tj)と三次元点の座標Xiが分かると、環境地図全体の再投影誤差を最小化したことになる。
次に、環境地図生成部15は、複数回の追跡に成功した特徴点を環境地図1001に登録する(S602)。ステップS602の処理は、例えば三次元再構成処理と、信頼度計測処理とを有する。三次元再構成処理とは、例えば特徴点の三次元座標を求める処理である。三次元座標の算出は、図4と同様に求めることができる。具体的には以下の(14)〜(16)式で求める。カメラ座標系において、画像における特徴点の座標が分かっているので視差Zと焦点距離fから測距情報が得られる((16)式)。測距情報が得られると、ピンホールカメラモデルにより、カメラ座標系のX座標とY座標を求めることができる((14)(15)式)。
Z:測距情報
xr:特徴点の右カメラの画像上の観測座標[pixel]
yr:特徴点の右カメラの画像上の観測座標[pixel]
b: ステレオカメラ50のベースラインの距離[mm]
d: 視差[pixel]
f: 焦点距離 [mm]
cx:画像のX方向の中心座標 [pixel]
cy:画像のY方向の中心座標 [pixel]
なお、ステレオカメラ50でなく単眼カメラである場合、2つの単眼カメラを用いてDLT(Direct Linear Transformation)法等を用いて三次元位置を求めることができる(DLT法を用いた手法については非特許文献2を参照)。
次に、上述した信頼度計測処理とは、再投影誤差のフィッシャー(Fisher)情報量の逆数を信頼度として、これが閾値より高い三次元点を信頼できる点として環境地図1001に登録する処理である。フィッシャー情報量は、確率変数がパラメータに関して持つ情報量のことである。なお、上述の閾値は、予め設定された値としてもよく、また環境地図1001への追加候補点の全点のうち、上位から所定数の点を環境地図1001に追加するようにしてもよい。上述したような追加する点の抽出条件は、処理毎に設定してもよい。つまり、S602の処理において、環境地図生成部15は、三次元再構成処理に成功し、かつ信頼度計測により信頼度が高い点のみを環境地図1001に登録する。
次に、環境地図生成部15は、環境地図1001にあるアウトライアの三次元点を削除する(S603)。アウトライアとは統計において他の値から大きく外れた値をいう。ステップS603の処理では、環境地図1001にある全点について、上述した信頼度を算出し、算出した信頼度が閾値より小さい点を環境地図1001から削除する。この閾値についても予め設定された値としてもよく、環境地図1001にある三次元点の全点のうち、下位から所定数の点を環境地図1001から削除してもよい。上述したような削除する点の抽出条件は、処理毎に設定してもよい。
<<S70 許容運動定義>>
次に、許容運動定義について説明する。許容運動定義とは、飛行物体100の運動を表す複数のパラメータセットのそれぞれについて、その時点で、どのパラメータセットが許容されるかが記述された情報である。
次に、許容運動定義について説明する。許容運動定義とは、飛行物体100の運動を表す複数のパラメータセットのそれぞれについて、その時点で、どのパラメータセットが許容されるかが記述された情報である。
図9は、許容運動定義部17が許容運動を定義する手順を示すフローチャート図の一例である。図9の処理は、運動パラメータIDの全てについて繰り返し実行される。ここでは運動パラメータIDの数をn個(0〜n−1)とする。運動パラメータIDは運動パラメータを区別するための識別情報である。識別情報とは、複数の対象からある特定の対象を一意的に区別するために用いられる名称、符号、文字列、数値又はこれらの組み合わせをいう。
図9の処理では、まず、許容運動定義部17が許容運動判定を行う(S701)。許容運動判定とは、許容できる運動かどうかを判定することをいうが詳細は図10にて説明される。
そして、許容できる運動である場合(S702のYes)、i番目の運動パラメータに関する運動を許容運動として登録する(S703)。
許容運動定義部17が有限の数の運動パラメータから、特徴点を追跡可能な運動パラメータを選択するので、実用的な時間内に許容できる運動を決定できる。
図10は、図9の許容運動判定の処理を説明するフローチャート図の一例である。許容運動判定の処理は、ある運動パラメータで飛行物体100が運動した場合に、環境地図に含まれる三次元点のうち最新の画像で撮像された特徴点が、撮像中(露光中)の画像で観測可能かを判定する処理である。露光時間が長ければ撮像中の画像での観測は困難になり、フレーム間隔が長ければ特徴点が大きく移動するので撮像中の画像での観測は困難になる。したがって、撮像中(露光中)の画像において、環境地図に含まれる点のうち最新の画像で撮像された点が撮像中(露光中)の画像でどのくらい移動するかを予測することで、撮像中(露光中)の画像で撮像できるかどうかを判定可能となる。図10では、一定数以上の特徴点が観測可能であると予想される運動パラメータ(運動)を許容運動であると判定する。
まず、許容運動定義部17は、記憶部1000から環境地図1001を読み出す(S7701)。
また、許容運動定義部17は、記憶部1000からカメラパラメータ1003を読み出す(S7702)。カメラパラメータ1003には露光時間とフレーム間隔が含まれている。
次に、許容運動定義部17は、最新の画像で撮像された特徴点かつ環境地図に含まれる特徴点の数をM個として、以下の処理を繰り返す。
まず、許容運動定義部17は、露光中の特徴点の移動距離を予測する(S7703)。微小時間の間に飛行物体100が、回転行列dRで表される回転を行い、平行移動ベクトルdtで表される移動を行う場合、画像上の移動量vは以下の(17)式で求めることができる。
回転行列dRは、最新の画像に撮像されている特徴点のワールド座標系の三次元座標をカメラ座標系に変換する回転行列Rの各要素の微小量である。
まず、回転行列dRと平行移動ベクトルdtは以下のように求める。移動***置計測部16は位置と姿勢を計測しているため、移動速度と各回転軸の回転速度を算出している。露光時間は既知なので、移動速度で飛行物体100が露光時間、移動する際の平行移動ベクトルdtを算出できる。同様に、各回転軸の回転速度は、ヨー角ψ、ピッチ角θ、ロール角φの時間的変化なので、移動***置計測部16に取って既知である。各回転軸の回転速度に露光時間を乗じれば露光時間における回転量が分かる。
そして、許容運動定義部17は、現在着目している運動パラメータで使用する運動を運動パラメータ使用運動1006から読み出す。そして、使用しない運動に対応する回転行列dRと平行移動ベクトルdtの要素をゼロに置き換える。例えば、上昇する場合は回転行列dRの各要素をゼロにしてよい。また、移動を伴わない姿勢変化では、平行移動ベクトルdtの各要素をゼロにしてよい。また、X方向(のみ)への移動であれば、移動の原理に基づく回転角以外は変化しないので、回転行列dRの要素のうち変化しない要素をゼロにしてよい。この場合、Y座標とZ座標は変化しないので、平行移動ベクトルdtうちY座標とZ座標の変化分をゼロにしてよい。このようにして、運動パラメータに応じて回転行列dRと平行移動ベクトルdtを算出できる。
許容運動定義部17は、このようにして求めた回転行列dRと平行移動ベクトルdtを(17)式に設定することで、露光時間における飛行物体100の画像上の移動量vを予測できる。
なお、回転行列dRと平行移動ベクトルdtの一方だけと露光時間から飛行物体100の画像上の移動量vを予測してもよい。
次に、許容運動定義部17は、露光時間における移動距離が閾値L1〔ピクセル数〕より小さいか否かを判定する(S7014)。露光時間中の移動距離が閾値L1以上の場合、被写体ブレ(モーションブラー)が発生する可能性が高いと判定する。すなわち、観測可能な特徴点でないと判定する。理論上は、1ピクセル以上の動きがある場合は被写体ブレが発生する。このため、閾値L1は1ピクセル(画素と画素の1ピッチ)となり、露光時間中の移動距離が1ピクセル未満の場合はステップS7014の判定がYesとなる。若干の被写体ブレを許容して数ピクセルとしてもよい。
次に、運動制約定義17部は、フレーム間隔における画像上の飛行物体100の移動距離を予測する(S7015)。フレームとフレームの間における画像上の移動体の運動をベクトルで表すものをオプティカルフローと呼ぶ。本実施形態では、フレーム間のオプティカルフローは、ステップS7013と同様に求められる。すなわち、露光時間の代わりにフレーム間隔を用いればよい。
許容運動定義部17は、このようにして求め得た回転行列dRと平行移動ベクトルdtを(17)式に設定することで、フレーム間隔における飛行物体100の画像上の移動距離を予測できる。
次に、運動制約定義は、フレーム間の移動距離が閾値L2〔ピクセル数〕より小さいか否かを判定する(S7016)。閾値L2は、特徴点追跡アルゴリズムの性能の限界に考慮して予め定められている。よって、閾値L2は、特徴点の追跡アルゴリズムによって異なることが好ましい。例えば、特徴点追跡アルゴリズムが事前に探索範囲を指定するブロックマッチングである場合、探索範囲を指定する画素サイズが閾値L2となる。特徴点追跡アルゴリズムにPyramidal KLT (又はLucas-Kanade法など)が用いられた場合を説明する。特徴点追跡のPyramidal(ピラミッド)とは、元の画像から解像度の異なる複数の画像を作成してそれぞれから特徴点を探索することをいう。また、KLTは、微小時間においては平行移動のみしか生じず、特徴量の変化がないことを仮定して特徴点を追跡することをいう。Pyramidal KLT (又はLucas-Kanade法など)が用いられた場合、最高レベル(最も解像度が高い)のピラミッド画像においてKLTのコストが算出される局所ウインドウのサイズが閾値L2となる。
また、特徴量記述が用いられた手法の場合、画像のほぼ全体が探索範囲となるので、ステレオカメラ50の撮像範囲を超えない範囲が閾値L2となる。こうすることで、特徴点の追跡の性能を超えない運動で飛行物体100が移動することができる。
なお、フレーム間の移動距離が閾値L2より小さくても、次のフレームで特徴点が画像範囲外に移動してしまうおそれがある。このため、許容運動定義部17は、最新の画像における特徴点の位置にオプティカルフローの移動ベクトルを加えた座標が、縦横ともに0より大きくかつ画像の幅と高さを超えない範囲に収まっているかを判定する。この範囲に収まっていない場合は、フレーム間の移動距離が閾値L2より小さくても観測可能な特徴点としては登録されない。
ステップS7016の判定がYesの場合、許容運動定義部17は特徴点を可観測特徴点として登録する(S7017)。
M個の特徴点について可観測特徴点かどうかの判定が行われると処理はステップS7018に進む。
許容運動定義部17は、可観測特徴点がK個以上あるか否かを判定する(S7018)。
ステップS7018の判定がYesの場合、許容運動定義部17は、図9で着目しているi番目の運動パラメータが許容運動であると判定する(S7019)。許容運動定義部17は、運動パラメータのうち許容運動であると判定された運動パラメータを許容運動マップに登録する。
<移動体制御部30の処理>
次に、図11を用いて移動体制御部30の処理について説明する。図11は、移動体制御部30の動作手順を示すフローチャート図の一例である。移動体制御部30は、N個の許容運動マップから最も経路コストが小さい運動を採用する。許容運動マップは、運動パラメータのうち許容運動として登録された運動パラメータである。すなわち、許容運動マップの運動パラメータの運動を飛行物体100が行った場合、次の画像にN個以上の可観測特徴点が得られると判定されている。
次に、図11を用いて移動体制御部30の処理について説明する。図11は、移動体制御部30の動作手順を示すフローチャート図の一例である。移動体制御部30は、N個の許容運動マップから最も経路コストが小さい運動を採用する。許容運動マップは、運動パラメータのうち許容運動として登録された運動パラメータである。すなわち、許容運動マップの運動パラメータの運動を飛行物体100が行った場合、次の画像にN個以上の可観測特徴点が得られると判定されている。
移動体制御部30は、各運動パラメータごとに予定移動先に到達するための経路コストを算出する(S110)。経路コストは時間、燃費、機体への負荷などである。本実施形態では説明の便宜上、経路コストが時間であるとして説明する。
図12は、経路コストの算出方法を説明する図の一例である。例えば、飛行物体100が移動する経路にA座標とB座標が登録されている場合、飛行物体100はA座標に到達したが次に予定移動先であるB座標に移動する必要がある。ここで、A座標からB座標に移動する運動パラメータが複数ある場合がある。通常は、直線で到達できる経路cを移動する運動パラメータを採用するが、本実施形態では、許容運動定義部17が経路cを移動する運動パラメータを許容しないことができる。したがって、本実施形態の飛行物体100は、位置の推定を継続することができる。
この場合、移動体制御部30はB座標に移動するため経路aや経路bを通過する別の運動パラメータで経路コストを算出する(S110)。
そして、許容されている全ての運動パラメータで経路コストを算出すると、移動体制御部30は最も経路コストが低い運動パラメータを採用する(S120)。
したがって、許容運動定義部17で定義された許容運動の運動パラメータのいずれかのうち、最も低い経路コストで移動する運動パラメータを採用できる。例えば、最も短い時間で移動する運動パラメータを採用できる。
なお、経路はA*アルゴリズムやダイクストラ法などを用いて自動生成してもよいし、ユーザが移動体に記録させたものでも良い。
以上説明したように、本実施形態の飛行物体100は、画像の撮像条件に基づいて、被写体ブレが起こらず、かつ、フレーム間で追跡が可能な特徴点が閾値以上の画像が撮像される場合に、着目している運動パラメータの運動が可能であると判定する。したがって、本実施形態の飛行物体100は位置と姿勢を継続して検出しやすい。また、位置と姿勢を継続して検出しやすい運動パラメータのうち、最も低い経路コストで移動する運動パラメータを採用するので、移動に必要な時間の増大を最小限に抑制できる。
<その他の適用例>
以上、本発明を実施するための最良の形態について実施例を用いて説明したが、本発明はこうした実施例に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形及び置換を加えることができる。
以上、本発明を実施するための最良の形態について実施例を用いて説明したが、本発明はこうした実施例に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形及び置換を加えることができる。
本実施形態では移動体として飛行物体100を例に説明したが、車両、移動式ロボット、船、潜水艦等の各種移動体で適用することができる。また、移動体に搭載されたスマートフォンなどの情報処理装置に適用することができる。
また、図10などの構成例は、飛行物体100による処理の理解を容易にするために、主な機能に応じて分割したものである。処理単位の分割の仕方や名称によって本願発明が制限されることはない。飛行物体100の処理は、処理内容に応じて更に多くの処理単位に分割することもできる。また、1つの処理単位が更に多くの処理を含むように分割することもできる。
また、上述した各実施形態で述べたフローチャートは、矛盾の無い限り順序を入れ替えてもよい。
なお、ステレオカメラ50は撮像装置の一例であり、移動***置計測部16は位置検出部の一例であり、カメラパラメータは撮像条件の一例である。位置計測部10は位置推定装置の一例であり、位置計測部10が行う処理や方法は位置推定方法の一例である。
10 位置計測部
11 画像特徴点抽出部
12 画像特徴点追跡部
13 深度計測部
14 撮像部制御部
15 環境地図生成部
16 移動***置計測部
17 許容運動定義部
30 移動体制御部
50 ステレオカメラ
100 飛行物体
11 画像特徴点抽出部
12 画像特徴点追跡部
13 深度計測部
14 撮像部制御部
15 環境地図生成部
16 移動***置計測部
17 許容運動定義部
30 移動体制御部
50 ステレオカメラ
100 飛行物体
Computer Vision and Pattern Recognition, 2005. CVPR 2005. IEEE Computer So-ciety Conference on, Vol. 2, pp.807 - 814, 2005.。
佐藤智和、"複数の動画像を用いたカメラパラメータ推定に基づく屋外環境の三次元モデル化に関する研究"、奈良先端科学技術大学院大学、博士論文、2003年3月24日。
Claims (10)
- 被写体までの距離情報を検出可能な撮像装置が撮像した周囲の画像から移動体の位置を推定すると共に周囲の地図を作成する位置推定装置であって、
前記撮像装置が撮像した前記画像から特徴点を抽出する画像特徴点抽出部と、
前記画像の前記特徴点を前記撮像装置が撮像した前記画像において追跡する画像特徴点追跡部と、
前記移動体の三次元座標、前記距離情報及び前記撮像装置のカメラパラメータを用いて、前記特徴点の三次元座標を含む前記地図を生成する環境地図生成部と、
前記特徴点の前記三次元座標を前記画像に投影した場合の投影座標と前記特徴点の座標の誤差、又は、前記特徴点の座標を前記三次元座標に変換した場合の変換座標と前記特徴点の前記三次元座標の誤差がより小さくなる前記移動体の位置と姿勢を検出する位置検出部と、
前記位置検出部が前記位置に基づき検出する前記移動体の移動速度又は前記姿勢に基づき検出する前記移動体の回転速度の少なくともいずれかと、前記画像の撮像条件に基づいて、前記特徴点を追跡可能な前記画像を撮像できる前記移動体の運動を定義する許容運動定義部と、を有する位置推定装置。 - 前記撮像条件は露光時間に関する情報を含み、
前記許容運動定義部は、前記移動体が前記移動速度及び前記回転速度で運動し、前記撮像装置が前記露光時間で前記画像を撮像した場合に、前記特徴点を追跡可能な前記運動を定義する請求項1に記載の位置推定装置。 - 前記許容運動定義部は、前記移動体が前記移動速度及び前記回転速度で運動し、前記撮像装置が前記露光時間で前記画像を撮像した場合に、前記画像における前記特徴点の移動量が1ピクセル未満である前記運動を定義する請求項2に記載の位置推定装置。
- 前記撮像条件は撮像間隔に関する情報を含み、
前記許容運動定義部は、前記移動体が前記移動速度及び前記回転速度で運動し、前記撮像装置が前記撮像間隔で次の前記画像を撮像した場合に、前記特徴点を追跡可能な前記運動を定義する請求項1〜3のいずれか1項に記載の位置推定装置。 - 前記許容運動定義部は、前記撮像間隔における前記特徴点の移動ベクトルを算出し、
前記移動ベクトルの長さが閾値よりも小さい場合に、前記特徴点を追跡可能であると判定する請求項4に記載の位置推定装置。 - 前記画像特徴点追跡部は、事前に設定された探索範囲の中でのみ前記特徴点の探索を行い、
前記許容運動定義部は前記移動ベクトルで移動した前記特徴点が前記探索範囲に含まれる場合、前記特徴点を追跡可能であると判定する請求項5に記載の位置推定装置。 - 前記画像特徴点追跡部は、Pyramidal KLTを用いて前記特徴点の追跡を行い、
前記許容運動定義部は、前記移動ベクトルで移動した前記特徴点がPyramidal KLTで利用する最高レベルのピラミッド画像におけるKLTの局所ウインドウ内にある場合、前記特徴点を追跡可能であると判定する請求項6に記載の位置推定装置。 - 前記許容運動定義部は、前記移動体が採用することが可能な運動を規定する複数の運動パラメータ及び前記運動パラメータに対応付けられた変化しうる自由度を記憶部から読み出し、
前記許容運動定義部は、前記運動パラメータごとに前記自由度を参照して、前記運動パラメータが規定する運動が、前記特徴点を追跡可能な前記画像を撮像できる運動であるかどうかを判定する請求項1〜7のいずれか1項に記載の位置推定装置。 - 被写体までの距離情報を検出可能な撮像装置が撮像した周囲の画像から移動体の位置を推定すると共に周囲の地図を作成する情報処理装置を、
前記撮像装置が撮像した前記画像から特徴点を抽出する画像特徴点抽出部と、
前記画像の前記特徴点を前記撮像装置が撮像した前記画像において追跡する画像特徴点追跡部と、
前記移動体の三次元座標、前記距離情報及び前記撮像装置のカメラパラメータを用いて、前記特徴点の三次元座標を含む前記地図を生成する環境地図生成部と、
前記三次元座標を前記画像に投影した場合の投影座標と前記特徴点の座標の誤差、又は、前記特徴点の座標を前記三次元座標に変換した場合の変換座標と前記三次元座標の誤差がより小さくなる前記移動体の位置と姿勢を検出する位置検出部と、
前記位置検出部が前記位置に基づき検出する前記移動体の移動速度又は前記姿勢に基づき検出する前記移動体の回転速度の少なくともいずれかと、前記画像の撮像条件に基づいて、前記特徴点を追跡可能な前記画像を撮像できる運動を定義する許容運動定義部、として機能させるためのプログラム。 - 被写体までの距離情報を検出可能な撮像装置が撮像した周囲の画像から移動体の位置を推定すると共に周囲の地図を作成する位置推定装置の位置推定方法であって、
画像特徴点抽出部が、前記撮像装置が撮像した前記画像から特徴点を抽出するステップと、
画像特徴点追跡部が、前記画像の前記特徴点を前記撮像装置が撮像した前記画像において追跡するステップと、
環境地図生成部が、前記移動体の三次元座標、前記距離情報及び前記撮像装置のカメラパラメータを用いて、前記特徴点の三次元座標を含む前記地図を生成するステップと、
位置検出部が、前記三次元座標を前記画像に投影した場合の投影座標と前記特徴点の座標の誤差、又は、前記特徴点の座標を前記三次元座標に変換した場合の変換座標と前記三次元座標の誤差がより小さくなる前記移動体の位置と姿勢を検出するステップと、
前記位置検出部が前記位置に基づき検出する前記移動体の移動速度又は前記姿勢に基づき検出する前記移動体の回転速度の少なくともいずれかと、前記画像の撮像条件に基づいて、許容運動定義部が、前記特徴点を追跡可能な前記画像を撮像できる運動を定義するステップと、を有する位置推定方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2016013860A JP6658001B2 (ja) | 2016-01-27 | 2016-01-27 | 位置推定装置、プログラム、位置推定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2016013860A JP6658001B2 (ja) | 2016-01-27 | 2016-01-27 | 位置推定装置、プログラム、位置推定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2017134617A true JP2017134617A (ja) | 2017-08-03 |
JP6658001B2 JP6658001B2 (ja) | 2020-03-04 |
Family
ID=59504968
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2016013860A Active JP6658001B2 (ja) | 2016-01-27 | 2016-01-27 | 位置推定装置、プログラム、位置推定方法 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP6658001B2 (ja) |
Cited By (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109325967A (zh) * | 2018-09-14 | 2019-02-12 | 腾讯科技(深圳)有限公司 | 目标跟踪方法、装置、介质以及设备 |
CN109753076A (zh) * | 2017-11-03 | 2019-05-14 | 南京奇蛙智能科技有限公司 | 一种无人机视觉追踪实现方法 |
WO2019093282A1 (ja) * | 2017-11-08 | 2019-05-16 | 川崎重工業株式会社 | 環境取得システム |
WO2019117078A1 (ja) * | 2017-12-15 | 2019-06-20 | 東芝ライフスタイル株式会社 | 電気掃除機 |
KR20190124515A (ko) * | 2018-04-26 | 2019-11-05 | 한국조선해양 주식회사 | 3d 지도 작성 시스템 |
CN110750094A (zh) * | 2018-07-23 | 2020-02-04 | 杭州海康威视数字技术股份有限公司 | 确定可移动设备的位姿变化信息的方法、装置和*** |
JP2020064339A (ja) * | 2018-10-15 | 2020-04-23 | 株式会社Uei | 地図作成装置、ロボット、地図作成方法及び地図作成プログラム |
JPWO2019111701A1 (ja) * | 2017-12-05 | 2020-11-26 | ソニー株式会社 | 制御装置、および制御方法、プログラム、並びに移動体 |
CN112417924A (zh) * | 2019-08-20 | 2021-02-26 | 北京地平线机器人技术研发有限公司 | 一种标志杆的空间坐标获取方法及装置 |
EP3761629A4 (en) * | 2018-02-26 | 2021-04-21 | Sony Corporation | INFORMATION PROCESSING DEVICE, INFORMATION PROCESSING PROCESS AND PROGRAM |
KR20210057964A (ko) * | 2019-11-13 | 2021-05-24 | 울산대학교 산학협력단 | 촬영장비를 구비한 이동체의 위치정보를 판단하는 방법 및 시스템 |
JP2021099384A (ja) * | 2019-12-19 | 2021-07-01 | キヤノン株式会社 | 情報処理装置、情報処理方法およびプログラム |
JP2021518558A (ja) * | 2018-03-19 | 2021-08-02 | アウトサイト | 物体の材料組成を識別するための方法およびシステム |
WO2023030062A1 (zh) * | 2021-09-01 | 2023-03-09 | 中移(成都)信息通信科技有限公司 | 一种无人机的飞行控制方法、装置、设备、介质及程序 |
JP7500515B2 (ja) | 2021-09-03 | 2024-06-17 | 株式会社東芝 | 被検体の検査に関連する処理装置、被検体の検査システム、被検体の検査に関連する処理方法、及び、被検体の検査に関連する処理プログラム |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2011209203A (ja) * | 2010-03-30 | 2011-10-20 | Sony Corp | 自己位置推定装置および自己位置推定方法 |
WO2015049717A1 (ja) * | 2013-10-01 | 2015-04-09 | 株式会社日立製作所 | 移動***置推定装置および移動***置推定方法 |
JP2016111414A (ja) * | 2014-12-03 | 2016-06-20 | コニカミノルタ株式会社 | 飛行体の位置検出システム及び飛行体 |
-
2016
- 2016-01-27 JP JP2016013860A patent/JP6658001B2/ja active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2011209203A (ja) * | 2010-03-30 | 2011-10-20 | Sony Corp | 自己位置推定装置および自己位置推定方法 |
WO2015049717A1 (ja) * | 2013-10-01 | 2015-04-09 | 株式会社日立製作所 | 移動***置推定装置および移動***置推定方法 |
JP2016111414A (ja) * | 2014-12-03 | 2016-06-20 | コニカミノルタ株式会社 | 飛行体の位置検出システム及び飛行体 |
Cited By (25)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109753076A (zh) * | 2017-11-03 | 2019-05-14 | 南京奇蛙智能科技有限公司 | 一种无人机视觉追踪实现方法 |
CN109753076B (zh) * | 2017-11-03 | 2022-01-11 | 南京奇蛙智能科技有限公司 | 一种无人机视觉追踪实现方法 |
WO2019093282A1 (ja) * | 2017-11-08 | 2019-05-16 | 川崎重工業株式会社 | 環境取得システム |
JP2019086419A (ja) * | 2017-11-08 | 2019-06-06 | 川崎重工業株式会社 | 環境取得システム |
TWI687870B (zh) * | 2017-11-08 | 2020-03-11 | 日商川崎重工業股份有限公司 | 環境取得系統以及環境取得方法 |
CN111417836A (zh) * | 2017-11-08 | 2020-07-14 | 川崎重工业株式会社 | 环境取得*** |
JPWO2019111701A1 (ja) * | 2017-12-05 | 2020-11-26 | ソニー株式会社 | 制御装置、および制御方法、プログラム、並びに移動体 |
US11592829B2 (en) | 2017-12-05 | 2023-02-28 | Sony Corporation | Control device and control method, program, and mobile object |
JP7180612B2 (ja) | 2017-12-05 | 2022-11-30 | ソニーグループ株式会社 | 制御装置、および制御方法、プログラム、並びに移動体 |
WO2019117078A1 (ja) * | 2017-12-15 | 2019-06-20 | 東芝ライフスタイル株式会社 | 電気掃除機 |
EP3761629A4 (en) * | 2018-02-26 | 2021-04-21 | Sony Corporation | INFORMATION PROCESSING DEVICE, INFORMATION PROCESSING PROCESS AND PROGRAM |
US11430204B2 (en) | 2018-02-26 | 2022-08-30 | Sony Corporation | Information processing device, information processing method, and program |
JP2021518558A (ja) * | 2018-03-19 | 2021-08-02 | アウトサイト | 物体の材料組成を識別するための方法およびシステム |
KR20190124515A (ko) * | 2018-04-26 | 2019-11-05 | 한국조선해양 주식회사 | 3d 지도 작성 시스템 |
KR102618865B1 (ko) * | 2018-04-26 | 2023-12-28 | 에이치디한국조선해양 주식회사 | 3d 지도 작성 시스템 |
CN110750094A (zh) * | 2018-07-23 | 2020-02-04 | 杭州海康威视数字技术股份有限公司 | 确定可移动设备的位姿变化信息的方法、装置和*** |
CN109325967A (zh) * | 2018-09-14 | 2019-02-12 | 腾讯科技(深圳)有限公司 | 目标跟踪方法、装置、介质以及设备 |
CN109325967B (zh) * | 2018-09-14 | 2023-04-07 | 腾讯科技(深圳)有限公司 | 目标跟踪方法、装置、介质以及设备 |
JP2020064339A (ja) * | 2018-10-15 | 2020-04-23 | 株式会社Uei | 地図作成装置、ロボット、地図作成方法及び地図作成プログラム |
CN112417924A (zh) * | 2019-08-20 | 2021-02-26 | 北京地平线机器人技术研发有限公司 | 一种标志杆的空间坐标获取方法及装置 |
KR20210057964A (ko) * | 2019-11-13 | 2021-05-24 | 울산대학교 산학협력단 | 촬영장비를 구비한 이동체의 위치정보를 판단하는 방법 및 시스템 |
KR102473202B1 (ko) * | 2019-11-13 | 2022-12-02 | 울산대학교 산학협력단 | 촬영장비를 구비한 이동체의 위치정보를 판단하는 방법 및 시스템 |
JP2021099384A (ja) * | 2019-12-19 | 2021-07-01 | キヤノン株式会社 | 情報処理装置、情報処理方法およびプログラム |
WO2023030062A1 (zh) * | 2021-09-01 | 2023-03-09 | 中移(成都)信息通信科技有限公司 | 一种无人机的飞行控制方法、装置、设备、介质及程序 |
JP7500515B2 (ja) | 2021-09-03 | 2024-06-17 | 株式会社東芝 | 被検体の検査に関連する処理装置、被検体の検査システム、被検体の検査に関連する処理方法、及び、被検体の検査に関連する処理プログラム |
Also Published As
Publication number | Publication date |
---|---|
JP6658001B2 (ja) | 2020-03-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6658001B2 (ja) | 位置推定装置、プログラム、位置推定方法 | |
CN112567201B (zh) | 距离测量方法以及设备 | |
US11127203B2 (en) | Leveraging crowdsourced data for localization and mapping within an environment | |
US9996936B2 (en) | Predictor-corrector based pose detection | |
JP6107081B2 (ja) | 画像処理装置、画像処理方法及びプログラム | |
CN109461208B (zh) | 三维地图处理方法、装置、介质和计算设备 | |
US10157478B2 (en) | Enabling use of three-dimensional locations of features with two-dimensional images | |
KR20150144729A (ko) | 그래디언트 기반 특징점을 이용한 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법 | |
JP2004213332A (ja) | キャリブレーション装置、キャリブレーション方法、キャリブレーション用プログラム、及び、キャリブレーション治具 | |
CN112258567A (zh) | 物体抓取点的视觉定位方法、装置、存储介质及电子设备 | |
JP2013187862A (ja) | 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム | |
CN105324792B (zh) | 用于估计移动元件相对于参考方向的角偏差的方法 | |
JP2020042819A (ja) | 運動ベクトル場の決定方法、運動ベクトル場の決定装置、機器、コンピュータ読み取り可能な記憶媒体及び車両 | |
JP7183085B2 (ja) | 移動体行動登録装置、移動体行動登録システム、移動体行動登録方法、移動体行動登録プログラム、及び移動体行動決定装置 | |
US11504608B2 (en) | 6DoF inside-out tracking game controller | |
JP2018173882A (ja) | 情報処理装置、方法、及びプログラム | |
CN110503684A (zh) | 相机位姿估计方法和装置 | |
US10977810B2 (en) | Camera motion estimation | |
WO2018134866A1 (ja) | カメラ校正装置 | |
JP2017182564A (ja) | 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム | |
WO2019186677A1 (ja) | ロボット位置姿勢推定・三次元計測装置 | |
KR101863647B1 (ko) | 3d 맵들에 대한 가설 라인 맵핑 및 검증 | |
JP5230354B2 (ja) | 位置特定装置及び異動建物検出装置 | |
JP6603993B2 (ja) | 画像処理装置、画像処理方法、画像処理システム、及びプログラム | |
JPWO2021111613A1 (ja) | 3次元地図作成装置、3次元地図作成方法、及び3次元地図作成プログラム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20190124 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20200107 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20200120 |
|
R151 | Written notification of patent or utility model registration |
Ref document number: 6658001 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |