JP5310285B2 - 自己位置推定装置及び自己位置推定方法 - Google Patents
自己位置推定装置及び自己位置推定方法 Download PDFInfo
- Publication number
- JP5310285B2 JP5310285B2 JP2009140836A JP2009140836A JP5310285B2 JP 5310285 B2 JP5310285 B2 JP 5310285B2 JP 2009140836 A JP2009140836 A JP 2009140836A JP 2009140836 A JP2009140836 A JP 2009140836A JP 5310285 B2 JP5310285 B2 JP 5310285B2
- Authority
- JP
- Japan
- Prior art keywords
- moving body
- self
- image
- position estimation
- feature point
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 29
- 238000003384 imaging method Methods 0.000 claims abstract description 85
- 230000008859 change Effects 0.000 claims abstract description 17
- 230000033001 locomotion Effects 0.000 claims description 91
- 238000011065 in-situ storage Methods 0.000 claims description 11
- 238000001514 detection method Methods 0.000 claims description 7
- 230000002093 peripheral effect Effects 0.000 abstract description 5
- 230000003287 optical effect Effects 0.000 description 59
- 230000007613 environmental effect Effects 0.000 description 17
- 238000012545 processing Methods 0.000 description 10
- 230000008569 process Effects 0.000 description 9
- 238000010586 diagram Methods 0.000 description 4
- 238000011156 evaluation Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
Images
Landscapes
- Image Analysis (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Studio Devices (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Description
これによって、ロボットを目的地に移動させる際に、不必要な観測及び距離画像計算処理を省略することで、効率的で短時間に移動経路計画を生成する。
本発明は、上記のような点を考慮したもので、特徴点がロストする可能性を低減し、自己位置推定を継続的に実施可能な自己位置推定の技術の提供を課題としている。
すなわち、周囲環境の物体を特定するための特徴点がロストする可能性を低減して、自己位置推定を継続的に実施可能となる。
図1は、本実施形態の移動体の外形を示す図である。図2は、移動体その他の構成を示す図である。
(構成)
まず移動体1の構成について説明する。
本実施形態の移動体1は、図1及び図2に示すように、左右両側に車輪2を配置した2輪構成を基本とする移動体1である。その左右2輪を駆動輪とし、その後側に補助輪3を備える。
また、駆動制御部5は、後述の制御コントローラ6からの指令に応じて駆動部4を制御することで、左右の車輪2を回転制御する。これによって、移動体1は、制御コントローラ6からの指令に応じた方向に進行する。ここで、走行の制御は、制御対象となる移動体1の機構に依存する。本実施形態では、駆動輪が2輪の移動体1の場合を想定しているので、左右の車輪2の平均回転数で移動速度を制御し、左右の車輪2の回転量の差で回転(旋回)を制御すればよい。
また、上記移動体1に対し、撮像装置10、及び回転駆動装置11を設ける。
移動状態取得手段6Aは、移動体1の移動情報(運動情報)を取得する。例えば、移動状態取得手段6Aは、移動体1の直進移動量(前後方向の移動量)及び回転量(旋回角度)を移動状態量として取得する。移動状態取得手段6Aは、取得した移動状態量を環境認識手段6Bに出力する。移動状態取得手段6Aは、例えば各車輪2の回転量を検出する上記回転数センサ7からの信号による各輪の回転量に基づき、移動体1の前後方向の移動量、または移動体1の進行方向Dmを算出する。
ここで、環境認識マップを特定する座標系の中心は、最初に撮像装置10により画像を撮像した場所とし、これを中心(原点)としたグローバル座標系を持つ。なお、座標系は、これに限定しない。移動体1の最初の位置が特定出来れば、他の位置に原点を持つ座標系であっても良い。また、環境認識マップには、周辺物体上のトラッキングが容易な点、つまり特徴点T(ランドマーク)についてのグローバル座標と、その特徴点Tを含む特徴点T周囲の画像を保存する。
図4に示すように、移動体1の移動前の特徴点Tの位置が画像上で角度θ1に位置し、移動体1の移動後の特徴点Tの位置が画像上で角度θ2に変化したとする。
この場合、図4から、下記式が成り立つ。
tanθ1 = L/W ・・・(1)
tanθ2 =(L −vt)/W ・・・(2)
tanθ1 −tanθ2 =vt/W ・・・(3)
ここで、
L :移動前の移動体1位置を基準とした特徴点Tの前後方向位置(縦方向位置)
W :移動前の移動体1位置を基準とした特徴点Tの横方向位置
v :移動体1の移動速度
t :移動時間
である。
W =vt /(tanθ1 −tanθ2) ・・・(4)
と記載出来る。
また、 (1)式のWを(3)式に代入して整理すると、
L =vt・tanθ1 /(tanθ1 −tanθ2) ・・・(5)
そして、上記(4)式及び(5)式から分かるように、特徴点Tについて、最初にどこに写っていたかの情報(θ1)と、移動体1の移動に伴う特徴点Tの変化((θ1−θ2))と、移動体1の移動量vtとから、移動体1の位置に対する特徴点Tの位置を演算することが出来る。
ここで、本実施形態において、「その場回転」とは、回転半径が上記環境移動量未満での回転(旋回)を指す。
また、地図生成のためには、通常、画像に存在する複数の特徴点Tについて位置の検出処理を行う。なお、2次元で自己位置を推定する場合には、2点以上の特徴点Tをトラッキング出来れば自己位置推定が可能である。3次元で自己位置を推定する場合には、3点以上の特徴点Tをトラッキング出来れば自己位置推定が可能である。
環境認識手段6Bの処理は、所定サンプリング周期で実施する。
まず、ステップS310にて、撮像装置10が撮像した画像を取得する。
次に、ステップS320にて、取得した画像に存在する特徴点Tを検出する。
ここで、画像上の特徴点Tとは、例えば、画像上で、上下方向、左右方向に差分を取ったときその変化量が大きい位置である。この特徴点Tは、撮像装置10と対象とする周辺物体の位置関係が変化したときに、その動きを画像処理により追従しやすいという特徴を持つ。取得した画像から特徴点Tを検出する一般的な特徴点T抽出の手法としては、SIFT特徴量、fast corner detection などを挙げることが出来る。
このように、所定サンプリング周期毎に連続して特徴点Tの移動をトラッキングしておく。
なお、今まで取得した移動体1の自己位置推定と相対距離によって、グローバル座標系における対象とする物体の位置を求めることが出来る。すなわち、移動体1の移動と周辺の観測を逐次繰り返し、過去の情報と相関を取ることで、地図をマッピングすることが出来る。
自己位置推定手段本体6Cは、上述のように環境認識手段6Bで構築した周囲物体の配置図情報を基に、撮像装置10から得られる画像上の物体の配置から、撮像装置10を搭載する移動体1の位置を推定する。すなわち、自己位置推定手段本体6Cは、既に構築した環境認識マップの情報に基づき、周囲の物体との相対位置関係から自己位置を推定する。この推定で使用するパラメータは、移動体1を平面移動に限定する場合は3次元(グローバル座標系における、例えばx, y, θ)である。ここでx、yは平面上のx方向及びy方向の値を、θは上下方向の角度である。また、ピッチング、ローリング運動も含めて推定する場合は6次元(グローバル座標系におけるx, y, z, θ,ι, κ)となる。ιはピッチング量、κはローリング量である。
先ずステップS410では、今までの自己位置推定結果に基づき、現在の撮像装置10位置から見た場合、環境認識マップに保存されている特徴点Tが画像上のどの位置に見えるかを算出する。これは環境認識マップ内で、以前行った撮像装置10位置と環境認識マップ内の特徴点Tとの、相対角度と相対距離から計算を行う。
次にステップS420では、撮像装置10から現在の画像を取得する。
次にステップS430では、ステップS410で画像内での位置を推定した特徴点Tが、現在の画像内のどこに存在するのか探索を行う。この探索もステップS330にて説明した一般的なトラッキング手法を採用すれば良い。
光軸保持手段6Eaは、移動体制御手段6Dからの信号に基づき、移動体1がその場回転していると判定すると、撮像装置10の光軸の向きがグローバル座標系において同一方向を保持するように、移動体1の移動に伴い回転駆動装置11を駆動制御する。すなわち、移動体1がその場回転を実施している場合は、移動体1がその場回転を実施している際に、その場回転開始時にグローバル座標系における撮像装置10が向いていた方向に光軸方向Drを保つように制御を実施する。その場回転か否かは、回転半径が環境移動量未満か否かで判定する。
光軸保持手段6Eaの制御は、光軸方向Drを一定に固定するように保持する制御に限定しない。図9に示すように、環境認識マップ内で既に位置が推定された特徴点Tが存在する場合は、少なくとも2点以上(平面移動に限定した場合)の特徴点Tが写っている範囲内で光軸方向Drを回転変位させても、自己位置推定を継続することが可能である。したがって、光軸保持手段6Eaによる光軸方向Drの制御を次のように制御しても良い。すなわち、画像上に、少なくとも2点以上の特徴点Tが画角内に入る光軸方向Drを算出し、その方向を新たにθ3とする。そして、角度調節のための出力を−a(θ2−θ3)と設定することで制御を実施する。
また、光軸復帰手段6Ebは、直進運動を検出すると、光軸の方向が初期方向(移動体1の進行方向Dm)からずれているか否かを判定する。初期方向からずれていると判定すると、初期方向に対する回転角度差が小さくなる方向に回転変位させる。
次に、移動体1の移動に伴うマッピング及び自己位置推定の処理部分について、図10を参照して説明する。
撮像装置10から現在の画像を取得する(ステップS510)。
次に、現在利用できるマップが存在することを確認する(ステップS520)。まだマップが存在しない場合には、最初の画像を取得してから、移動体1が環境移動量以上の距離を移動したか判定する。(ステップS530)。環境移動量以上の距離を移動していない場合には、撮像装置10から現在の画像を取得する(ステップS510)。
ここで、環境移動量以上の距離を移動したか否かを判定しているのは、マップを初期化する際に行うMotion Stereoによる周辺物体の距離計測を行う際の精度を一定以上に保つためである。環境移動量以上の距離を移動した否かの判定は、移動状態取得手段6Aの取得した直進移動量によって判定する。
そして、環境移動量以上の距離を移動したと判定した場合には、環境認識手段6Bにおいて、環境認識による環境認識マップの更新を実施する。
また、所定サンプリング周期毎に、自己位置推定手段本体6Cにおいて、既に作成した環境認識マップ上での移動体1位置を推定する(ステップS570)。
移動体1の自己位置の移動量の判定は、例えば自己位置推定値を用いる。移動体1の移動状態判定手段による判定や移動体制御手段6Dの制御指令値に基づき判定しても良い。
ただし、自己位置推定手段の結果を用いて、その場回転か否かを判定することで構成を単純化できる。
ここで、回転駆動装置11、光軸方向制御手段6E(光軸保持手段6Ea、光軸復帰手段6Eb)は、撮像方向制御手段を構成する。ステップS320は特徴点検出手段を構成する。環境認識手段6B及び自己位置推定手段本体6Cは、自己位置推定手段を構成する。
(1) 撮像装置10は、移動体1に搭載され且つ移動体周囲を撮像して画像を取得する。特徴点検出手段は、撮像装置が取得した画像に存在する特徴点を検出する。移動状態取得手段6Aは、上記移動体の移動状態を取得する。自己位置推定手段は、移動体の移動に伴う画像上での上記特徴点の変化から移動体周囲の物体の位置を検出することで、移動体周囲の物体に対する自己位置を推定する。撮像方向制御手段は、移動体がその場回転していない場合には上記撮像方向を、移動体上の座標系において予め定められた所定の方向である初期方向に制御し、移動体がその場回転している場合には上記撮像方向を、移動体がその場回転する前に取得した画像に存在した特徴点の少なくとも一部を撮像可能な向きに制御する。
すなわち、周囲環境の物体を特定するための特徴点Tをロストする可能性を低減して、自己位置推定を継続的に実施可能となる。
すなわち、光軸方向Drを一定方向に固定するように制御する。
この結果、撮像装置10の光軸方向Drを制御するための計算コストを一定に抑えることが可能になる。
例えば、撮像方向制御手段は、上記撮像方向が初期方向と異なる場合に、移動体1の直進運動を検知すると、移動体1に対する撮像方向を初期方向に変化させる。
これによって、その場回転に対応するために撮像方向が移動体1に対する初期方向がずれても、自己位置推定を継続しながら、光軸方向Drを進行方向Dmに向けることができる。
なお、光軸方向Drの初期方向は、移動体1の進行方向Dmと同方向に向けておくことが好ましい。移動体1の進行方向Dmの情報を一番取得したいからである。
(4)撮像方向制御手段は、取得している画像上の特徴点Tの変化に基づき、移動体1の直進運動を検知する。
移動体1に直進成分を伴う移動が発生したことを撮像装置10による自己位置推定を用いて判定することで、非常にシンプルな構成で機能を実現することができる。
6 制御コントローラ
6A 移動状態取得手段
6B 環境認識手段(自己位置推定手段)
6C 自己位置推定手段本体(自己位置推定手段)
6D 移動体制御手段
6E 光軸方向制御手段
6Ea 光軸保持手段(撮像方向制御手段)
6Eb 光軸復帰手段(撮像方向制御手段)
7 回転数センサ
10 撮像装置
11 回転駆動装置
Dm 進行方向
Dr 光軸方向(撮像方向)
T 特徴点
Claims (5)
- 移動体の自己位置を推定する自己位置推定装置であって、
移動体に搭載され且つ移動体周囲を撮像して画像を取得する撮像装置と、
移動体に対する撮像装置の撮像方向を制御する撮像方向制御手段と、
撮像装置が取得した画像に存在する特徴点を検出する特徴点検出手段と、
上記移動体の移動状態を取得する移動状態取得手段と、
移動体の移動に伴う画像上での上記特徴点の変化から移動体周囲の物体の位置を検出することで、移動体周囲の物体に対する自己位置を推定する自己位置推定手段と、を備え、
上記撮像方向制御手段は、移動体がその場回転していない場合には上記撮像方向を、移動体上の座標系において予め定められた所定の方向である初期方向に制御し、移動体がその場回転している場合には上記撮像方向を、移動体がその場回転する前に取得した画像に存在した特徴点の少なくとも一部を撮像可能な向きに制御することを特徴とする自己位置推定装置。 - 撮像方向制御手段は、移動体がその場回転している場合には上記撮像方向を、グローバル座標系において、移動体がその場回転する前の方向に固定されるように制御することを特徴とする請求項1に記載した自己位置推定装置。
- 上記撮像方向制御手段は、移動体がその場回転した後、移動体の直進運動を検知すると、移動体に対する撮像方向を上記初期方向に制御することを特徴とする請求項1または2のいずれかに記載の自己位置推定装置。
- 撮像方向制御手段は、取得している画像上の特徴点の変化に基づき、移動体の直進運動を検知することを特徴とする請求項3に記載した自己位置推定装置。
- 移動体に搭載した撮像装置により撮像した画像に存在する特徴点を検出し、移動体の移動に伴う画像上での上記特徴点の変化から、移動体周囲の物体の位置を検出することにより移動体周囲の物体に対する自己位置を推定し、
上記移動体がその場回転していない場合には上記撮像装置の撮像方向を、移動体上の座標系において予め定められた所定の方向である初期方向とし、上記移動体がその場回転している場合には上記撮像装置の撮像方向を、移動体がその場回転する前に取得した画像に存在した特徴点の少なくとも一部を撮像可能な向きとすることを特徴とする自己位置推定方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2009140836A JP5310285B2 (ja) | 2009-06-12 | 2009-06-12 | 自己位置推定装置及び自己位置推定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2009140836A JP5310285B2 (ja) | 2009-06-12 | 2009-06-12 | 自己位置推定装置及び自己位置推定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2010288112A JP2010288112A (ja) | 2010-12-24 |
JP5310285B2 true JP5310285B2 (ja) | 2013-10-09 |
Family
ID=43543490
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2009140836A Expired - Fee Related JP5310285B2 (ja) | 2009-06-12 | 2009-06-12 | 自己位置推定装置及び自己位置推定方法 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5310285B2 (ja) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10393515B2 (en) | 2016-01-20 | 2019-08-27 | Mitsubishi Electric Corporation | Three-dimensional scanner and measurement assistance processing method for same |
Families Citing this family (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6003673B2 (ja) * | 2013-01-23 | 2016-10-05 | 株式会社デンソー | 3次元位置推定装置、車両制御装置、および3次元位置推定方法 |
DE112014002943T5 (de) * | 2013-06-19 | 2016-03-10 | Mitsubishi Electric Corporation | Verfahren zur Registrierung von Daten unter Verwendung eines Satzes von Grundelementen |
JP6319736B2 (ja) * | 2013-09-19 | 2018-05-09 | 株式会社Ihi | センサの指向制御方法と装置 |
JP2016045874A (ja) | 2014-08-26 | 2016-04-04 | ソニー株式会社 | 情報処理装置、情報処理方法、及びプログラム |
JP6288060B2 (ja) | 2015-12-10 | 2018-03-07 | カシオ計算機株式会社 | 自律移動装置、自律移動方法及びプログラム |
JP6311695B2 (ja) | 2015-12-16 | 2018-04-18 | カシオ計算機株式会社 | 自律移動装置、自律移動方法及びプログラム |
JP6323439B2 (ja) | 2015-12-17 | 2018-05-16 | カシオ計算機株式会社 | 自律移動装置、自律移動方法及びプログラム |
JP6187623B1 (ja) | 2016-03-14 | 2017-08-30 | カシオ計算機株式会社 | 自律移動装置、自律移動方法及びプログラム |
JP6846897B2 (ja) * | 2016-03-24 | 2021-03-24 | パナソニック インテレクチュアル プロパティ コーポレーション オブ アメリカPanasonic Intellectual Property Corporation of America | 位置指示方法、位置指示装置、自走式装置及びプログラム |
JP2018163614A (ja) * | 2017-03-27 | 2018-10-18 | カシオ計算機株式会社 | 自律移動装置、自律移動方法およびプログラム |
JP2019197350A (ja) * | 2018-05-09 | 2019-11-14 | 株式会社日立製作所 | 自己位置推定システム、自律移動システム及び自己位置推定方法 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4161377B2 (ja) * | 1996-12-09 | 2008-10-08 | ソニー株式会社 | 移動体の撮影装置 |
JP2000242330A (ja) * | 1999-02-23 | 2000-09-08 | Mitsubishi Heavy Ind Ltd | 自動操舵移動体およびその移動体の操舵補正装置、並びに移動体の操舵補正方法 |
JP2002296010A (ja) * | 2001-03-29 | 2002-10-09 | Fujitsu Ltd | 画像を用いた自己位置同定方法 |
JP2007316966A (ja) * | 2006-05-26 | 2007-12-06 | Fujitsu Ltd | 移動ロボット、その制御方法及びプログラム |
-
2009
- 2009-06-12 JP JP2009140836A patent/JP5310285B2/ja not_active Expired - Fee Related
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10393515B2 (en) | 2016-01-20 | 2019-08-27 | Mitsubishi Electric Corporation | Three-dimensional scanner and measurement assistance processing method for same |
DE112016006262B4 (de) | 2016-01-20 | 2023-05-04 | Mitsubishi Electric Corporation | Dreidimensionaler Scanner und Verarbeitungsverfahren zur Messunterstützung für diesen |
Also Published As
Publication number | Publication date |
---|---|
JP2010288112A (ja) | 2010-12-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5310285B2 (ja) | 自己位置推定装置及び自己位置推定方法 | |
CN106774301B (zh) | 一种避障跟随方法和电子设备 | |
WO2019128070A1 (zh) | 目标跟踪方法及装置、移动设备及存储介质 | |
JP6711138B2 (ja) | 自己位置推定装置、及び、自己位置推定方法 | |
WO2020258721A1 (zh) | 智能巡航车导航方法及系统 | |
KR100772912B1 (ko) | 절대 방위각을 이용한 로봇 및 이를 이용한 맵 작성 방법 | |
KR100901311B1 (ko) | 자율이동 플랫폼 | |
JP5105596B2 (ja) | 自律走行移動体の走行経路決定用地図作成装置及び走行経路決定用地図作成方法 | |
JP4738472B2 (ja) | 障害物回避機能を有する移動制御装置 | |
JP2009031884A (ja) | 自律移動体、自律移動体におけるマップ情報作成方法および自律移動体における移動経路特定方法 | |
JP2009205226A (ja) | 自律移動ロボット、自己位置推定方法、環境地図の生成方法、環境地図の生成装置、及び環境地図のデータ構造 | |
JP2011209845A (ja) | 自律移動体、自己位置推定方法、地図情報作成システム | |
JP2009096335A (ja) | 脚型ロボット | |
JP4670807B2 (ja) | 移動経路作成方法、自律移動体および自律移動体制御システム | |
JP2008139035A (ja) | 3次元環境計測装置及びそれを備えた移動ロボット | |
JP2008142841A (ja) | 移動ロボット | |
US20240286422A1 (en) | Printing systems | |
JP5187757B2 (ja) | 無人移動体システム | |
JP2009174898A (ja) | 移動体および環境情報作成方法 | |
JP6962027B2 (ja) | 移動車両 | |
JP7002791B2 (ja) | 情報処理装置および移動ロボット | |
KR100784125B1 (ko) | 단일 카메라를 이용한 이동 로봇의 랜드 마크의 좌표 추출방법 | |
Jensen et al. | Laser range imaging using mobile robots: From pose estimation to 3d-models | |
JP2009006984A (ja) | 脚車輪型ロボット | |
Chang et al. | Reconstruction of 3D contour with an active laser‐vision robotic system |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
RD04 | Notification of resignation of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7424 Effective date: 20100917 |
|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20120424 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20130528 |
|
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: 20130604 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20130617 |
|
R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
LAPS | Cancellation because of no payment of annual fees |