JP2010288112A - 自己位置推定装置及び自己位置推定方法 - Google Patents

Abstract

【課題】周囲物体を特定する特徴点がロストする可能性を低減し、自己位置推定を継続的に実施可能な自己位置推定の技術の提供する。
【解決手段】移動体１に搭載した撮像装置１０により撮像する画像中の特徴点を検出しつつ、移動体１の移動に伴う画像上での上記特徴点の変化から移動体周囲の物体の位置を検出することで、自己位置を推定する。このとき、上記移動体１の動作がその場回転する場合には、移動体がその場回転する前に取得した画像中に位置する特徴点の少なくとも一部を撮像可能な向きに、上記撮像方向を保持する。
【選択図】 図３

Description

本発明は、移動しながら周囲物体に対する自己位置を推定する自己位置推定の技術に関し、特に、環境地図生成と自己位置推定の両方の処理を行うことが可能なＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）の手法を用いた自己位置推定の技術に関する。

特許文献１は、自己位置推定を行う移動型ロボットを開示している。すなわち、経路計画装置の障害物地図更新部が、障害物グリッド算出部からの障害物情報と現在位置及び目的地情報を受け取り、障害物地図を作成（更新）する。このとき、視線制御部は、ロボット装置の視線が目的地方向を向くように制御する。経路計画部は、障害物地図更新部が作成した障害物地図を基に、未観測領域と自由空間領域とを移動可能領域とみなして経路を計画する。経路評価部は、経路計画部で計画した経路を評価し、その評価結果に基づいて経路を経路計画部から出力する。若しくは、経路評価部は、生成不能であったとの結果を出力したり、視線制御部による視線制御を調整したりする。
これによって、ロボットを目的地に移動させる際に、不必要な観測及び距離画像計算処理を省略することで、効率的で短時間に移動経路計画を生成する。

特開２００３−２６６３４５号公報

周囲物体を特定する特徴点（ランドマーク）を途中でロストした場合、ユーザーが目的地を再設定することで回避出来る。しかし、移動体が自律走行するような場合にあっては、ユーザーが適切に目的地を再設定することが困難な場合がある。
本発明は、上記のような点を考慮したもので、特徴点がロストする可能性を低減し、自己位置推定を継続的に実施可能な自己位置推定の技術の提供を課題としている。

上記課題を解決するために、本発明は、移動体に搭載した撮像装置により撮像する画像中の特徴点を検出しつつ、移動体の移動に伴う画像上での上記特徴点の変化から移動体周囲の物体の位置を検出することで、自己位置を推定する。このとき、上記移動体がその場回転していない場合には上記撮像装置の撮像方向を、移動体上の座標系において予め定められた所定の方向である初期方向とし、上記移動体がその場回転している場合には上記撮像装置の撮像方向を、移動体がその場回転する前に取得した画像に存在した特徴点の少なくとも一部を撮像可能な向きとする。

移動体の移動状態（運動）がその場回転の場合には、特徴点で特定する対象物までの視差が発生しないか発生する視差が小さい。これに対し、本発明によれば、移動体がその場回転の場合には、移動体がその場回転する前に取得した画像中に位置する特徴点が存在する方向、つまり環境認識が実施された方向に画像の撮像方向を制御する。これによって、環境認識を更新できない移動状態が移動体に対し一時的に発生しても、自己位置推定を継続することができる。
すなわち、周囲環境の物体を特定するための特徴点がロストする可能性を低減して、自己位置推定を継続的に実施可能となる。

本発明に基づく実施形態に係る移動体を示す図である。 本発明に基づく実施形態に係る移動体の構成を説明する構成図である。 本発明に基づく実施形態に係る制御コントローラの構成を説明する図である。 Ｍｏｔｉｏｎ Ｓｔｅｒｅｏを説明する図である。 環境認識手段の処理を説明する図である。 自己位置推定手段本体の処理を説明する図である。 光軸方向制御手段の構成を説明する図である。 その場回転を説明する平面図である。 撮像範囲と特徴点の関係を示す平面図である。 地図を作成しながら自己位置推定の処理を説明する図である。

次に、本発明の実施形態を図面を参照して説明する。
図１は、本実施形態の移動体の外形を示す図である。図２は、移動体その他の構成を示す図である。
（構成）
まず移動体１の構成について説明する。
本実施形態の移動体１は、図１及び図２に示すように、左右両側に車輪２を配置した２輪構成を基本とする移動体１である。その左右２輪を駆動輪とし、その後側に補助輪３を備える。

その移動体１にはまた、駆動部４、及び駆動部４を制御する駆動制御部５を搭載する。駆動部４は、モータなどのアクチュエータからなり、アクチュエータの発生するトルクを駆動トルクとして左右の車輪２に伝達する。その伝達した駆動トルクによって車輪２を回転駆動する。すなわち、この移動体１は、左右の車輪２を駆動することで走行制御が可能となる。なお、左右の車輪２に回転数差を付けることで、移動体１を所定角度だけ回転（旋回）させて移動体１の進行方向Ｄｍを変更する。すなわち、直進運動と回転運動を組み合わせて使用することで、自由に走行可能となっている。

ここで、移動体１として、所定の面（床面や天井面など）に沿って移動するロボットを含む、様々な移動体１を対象とすることが可能である。また、車輪で走行制御する移動体１の場合において、４輪以上の車輪を有する車両であっても良い。
また、駆動制御部５は、後述の制御コントローラ６からの指令に応じて駆動部４を制御することで、左右の車輪２を回転制御する。これによって、移動体１は、制御コントローラ６からの指令に応じた方向に進行する。ここで、走行の制御は、制御対象となる移動体１の機構に依存する。本実施形態では、駆動輪が２輪の移動体１の場合を想定しているので、左右の車輪２の平均回転数で移動速度を制御し、左右の車輪２の回転量の差で回転（旋回）を制御すればよい。

なお、各車輪２には、回転数センサ７を備える。回転数センサ７は、例えば車輪２の回転量を検出するエンコーダーで構成する。回転数センサ７は、検出信号を駆動制御部５に出力し、駆動制御部５は、回転数センサ７の検出信号によって各駆動部４への指令値をフィードバック制御する。
また、上記移動体１に対し、撮像装置１０、及び回転駆動装置１１を設ける。

撮像装置１０は、移動体１の周囲の環境に存在する物体情報を取得する装置である。撮像装置１０は、カメラや光学センサーアレイ等の画像取得手段を備え、画像取得手段が撮像した画像情報を制御コントローラ６に出力する。上記画像取得手段の光軸方向Ｄｒ（撮像方向）は、初期方向として、移動体１の進行方向Ｄｍを向くように設定してある。すなわち、画像取得手段の光軸方向は通常時には（後述する、移動体がその場回転している場合以外の場合には）、移動体から予め定められた所定の方向、言い換えれば移動体上に固定された座標系における所定の方向である初期方向を向くように制御されている。このように初期方向を設定するのは、移動体１の進行方向Ｄｍの情報が一番取得したいからである。

上記撮像装置１０は、軸を上下に向けた回転軸Ｐ廻りに回転変位可能に移動体１に取り付ける。さらに、上記撮像装置１０は、回転駆動装置１１によって、上記回転軸Ｐ廻りに回転変位することで、光軸方向Ｄｒ（撮像方向）が走行体の走行状態とは独立に向きを調整することが可能となっている。本実施形態では、上記回転軸Ｐを、走行体の重心位置に設定している。上記回転駆動装置１１は、例えば、回転軸Ｐに固定した第１歯車と、その第１歯車に噛合する第２歯車と、その第２歯車を回転するサーボモータとから構成する。この回転駆動装置１１を制御することで、移動体の向きに対して撮像装置１０の光軸方向Ｄｒを調整することが可能となる。回転駆動装置１１は、制御コントローラ６からの指令によって、移動体１の進行方向Ｄｍに対する撮像装置１０の光軸方向Ｄｒ（撮像方向）を変更する。

上記制御コントローラ６は、移動状態取得手段６Ａ、環境認識手段６Ｂ、自己位置推定手段本体６Ｃ、移動体制御手段６Ｄ、及び光軸方向制御手段６Ｅを備える。
移動状態取得手段６Ａは、移動体１の移動情報（運動情報）を取得する。例えば、移動状態取得手段６Ａは、移動体１の直進移動量（前後方向の移動量）及び回転量（旋回角度）を移動状態量として取得する。移動状態取得手段６Ａは、取得した移動状態量を環境認識手段６Ｂに出力する。移動状態取得手段６Ａは、例えば各車輪２の回転量を検出する上記回転数センサ７からの信号による各輪の回転量に基づき、移動体１の前後方向の移動量、または移動体１の進行方向Ｄｍを算出する。

環境認識手段６Ｂは、撮像装置１０が撮像した画像及び移動状態取得手段６Ａが取得した移動体１の移動状態に基づき、周囲環境に存在する障害物等の物体の目印となるランドマークの配置を計測する。自己位置推手段本体６Ｃは、環境認識手段６Ｂにより計測された環境に対する自己位置を推定する。移動体制御手段６Ｄは、移動体１の移動を制御する。光軸方向制御手段６Ｅは、撮像装置１０の光軸方向Ｄｒを制御する。

次に、上記環境認識手段６Ｂについて説明する。環境認識手段６Ｂは、移動体１の移動に伴う画像上での特徴点Ｔの変化から、移動体１の周辺に存在する物体までの相対位置を算出し、移動体１周辺の物体配置図（環境認識マップ）を構成する手段である。
ここで、環境認識マップを特定する座標系の中心は、最初に撮像装置１０により画像を撮像した場所とし、これを中心（原点）としたグローバル座標系を持つ。なお、座標系は、これに限定しない。移動体１の最初の位置が特定出来れば、他の位置に原点を持つ座標系であっても良い。また、環境認識マップには、周辺物体上のトラッキングが容易な点、つまり特徴点Ｔ（ランドマーク）についてのグローバル座標と、その特徴点Ｔを含む特徴点Ｔ周囲の画像を保存する。

また、移動体１の移動に伴う画像の変化から、移動体１周辺に存在する物体までの相対位置を算出する手法としては、例えばＭｏｔｉｏｎ Ｓｔｅｒｅｏ方式を採用すれば良い。Ｍｏｔｉｏｎ Ｓｔｅｒｅｏ方式は、図２に示すように、移動体１の移動距離に応じて、画面上での物体を特定する特徴点Ｔの移動から当該物体（の特徴点Ｔ）の位置を計測する手法である。

このＭｏｔｉｏｎ Ｓｔｅｒｅｏ方式について説明する。
図４に示すように、移動体１の移動前の特徴点Ｔの位置が画像上で角度θ１に位置し、移動体１の移動後の特徴点Ｔの位置が画像上で角度θ２に変化したとする。
この場合、図４から、下記式が成り立つ。
ｔａｎθ１ ＝ Ｌ／Ｗ ・・・（１）
ｔａｎθ２ ＝（Ｌ −ｖｔ）／Ｗ ・・・（２）
ｔａｎθ１ −ｔａｎθ２ ＝ｖｔ／Ｗ ・・・（３）
ここで、
Ｌ ：移動前の移動体１位置を基準とした特徴点Ｔの前後方向位置（縦方向位置）
Ｗ ：移動前の移動体１位置を基準とした特徴点Ｔの横方向位置
ｖ ：移動体１の移動速度
ｔ ：移動時間
である。

そして、（３）式は、
Ｗ ＝ｖｔ ／（ｔａｎθ１ −ｔａｎθ２） ・・・（４）
と記載出来る。
また、 （１）式のＷを（３）式に代入して整理すると、
Ｌ ＝ｖｔ・ｔａｎθ１ ／（ｔａｎθ１ −ｔａｎθ２） ・・・（５）
そして、上記（４）式及び（５）式から分かるように、特徴点Ｔについて、最初にどこに写っていたかの情報（θ１）と、移動体１の移動に伴う特徴点Ｔの変化（（θ１−θ２））と、移動体１の移動量ｖｔとから、移動体１の位置に対する特徴点Ｔの位置を演算することが出来る。

ここで、特徴点Ｔの位置情報θ１、θ２や移動量ｖｔ等には測定誤差が含まれる。このため、所定の精度をもって特徴点Ｔの位置を取得するためには、角度変化（θ１−θ２）が所定角度以上変化するだけの移動量ｖｔを確保する必要がある。このため、本実施形態では、角度θ１及びθ２によらず所定の精度を確保する目的で、環境観測をするための移動体１の観測移動量として１ｍに設定している。この１ｍは一例であり、特徴点Ｔの位置情報の取得精度などや、目的とする地図の精度などに基づき、適宜設定すればよい。すなわち、画像取得手段や環境（例えば想定される移動体１と周囲環境物体との距離など）に基づき、特徴点Ｔの変化が、物体位置を検出に対するある程度の精度を保てるだけ変化出来る移動体１の距離を設定する。この移動体１の距離を本明細書では環境移動量と呼ぶ。この環境移動量は、想定される環境物体への距離によって変更したり、画像上での特徴点の変化量に基づき最適値を学習して変更するようにしても良い。

なお、「その場回転」のように回転半径が小さい移動の場合は、対象までの視差が発生しないか小さいため地図の更新ができず、自己位置推定を継続することができなくなることがある（ロストする）。すなわち、その場回転の場合には、回転に伴い、画像上の特徴点Ｔの位置情報は変化するが移動体１の移動が無いか小さいため、自己位置の推定が困難となる。
ここで、本実施形態において、「その場回転」とは、回転半径が上記環境移動量未満での回転（旋回）を指す。
また、地図生成のためには、通常、画像に存在する複数の特徴点Ｔについて位置の検出処理を行う。なお、２次元で自己位置を推定する場合には、２点以上の特徴点Ｔをトラッキング出来れば自己位置推定が可能である。３次元で自己位置を推定する場合には、３点以上の特徴点Ｔをトラッキング出来れば自己位置推定が可能である。

次に、環境認識手段６Ｂの処理例を、図５を参照して説明する。
環境認識手段６Ｂの処理は、所定サンプリング周期で実施する。
まず、ステップＳ３１０にて、撮像装置１０が撮像した画像を取得する。
次に、ステップＳ３２０にて、取得した画像に存在する特徴点Ｔを検出する。
ここで、画像上の特徴点Ｔとは、例えば、画像上で、上下方向、左右方向に差分を取ったときその変化量が大きい位置である。この特徴点Ｔは、撮像装置１０と対象とする周辺物体の位置関係が変化したときに、その動きを画像処理により追従しやすいという特徴を持つ。取得した画像から特徴点Ｔを検出する一般的な特徴点Ｔ抽出の手法としては、ＳＩＦＴ特徴量、ｆａｓｔ ｃｏｒｎｅｒ ｄｅｔｅｃｔｉｏｎ などを挙げることが出来る。

次に、ステップＳ３３０では、先に撮像した画像上の特徴点Ｔが、現在の画像のどこに移動したのかを計測する（トラッキング）。このトラッキング処理も手法が多く存在する。一般的には、先に撮像した画像での特徴点Ｔ位置の周辺を探索範囲とし、ＳＳＤ、ＳＡＤなどの手法を用いて、先に撮像した画像での特徴点Ｔをテンプレートとし探索範囲内の画像とのマッチング度合いを計測する。ここで最もマッチング度合いの高かった場所を、現在の画像上での特徴点Ｔ位置とする。
このように、所定サンプリング周期毎に連続して特徴点Ｔの移動をトラッキングしておく。

次に、ステップＳ３４０では、地図を更新してから、移動体１の直進移動距離が予め設定した環境移動量以上となったか否かを判定する。移動体１の直進移動距離が環境移動量未満であれば、そのまま復帰する。一方、移動体１の直進移動距離が環境移動量以上となったと判定した場合には、ステップＳ３５０に移行する。移動体１の直進移動距離は、移動状態取得手段６Ａから取得すればよい。

ステップＳ３５０では、各特徴点Ｔに対し、移動前の画像上での特徴点Ｔの位置情報、画面上の特徴点Ｔの移動距離、及び移動体１の移動距離から、上記Ｍｏｔｉｏｎ Ｓｔｅｒｅｏ方式等を使用して、特徴点Ｔで特定される周辺物体と移動体１との相対距離（Ｌ、Ｗ）を演算し、演算結果を環境認識マップに保存する。これによって地図を更新する。
なお、今まで取得した移動体１の自己位置推定と相対距離によって、グローバル座標系における対象とする物体の位置を求めることが出来る。すなわち、移動体１の移動と周辺の観測を逐次繰り返し、過去の情報と相関を取ることで、地図をマッピングすることが出来る。

次に、自己位置推定手段本体６Ｃについて説明する。
自己位置推定手段本体６Ｃは、上述のように環境認識手段６Ｂで構築した周囲物体の配置図情報を基に、撮像装置１０から得られる画像上の物体の配置から、撮像装置１０を搭載する移動体１の位置を推定する。すなわち、自己位置推定手段本体６Ｃは、既に構築した環境認識マップの情報に基づき、周囲の物体との相対位置関係から自己位置を推定する。この推定で使用するパラメータは、移動体１を平面移動に限定する場合は３次元（グローバル座標系における、例えばｘ， ｙ， θ）である。ここでｘ、ｙは平面上のｘ方向及びｙ方向の値を、θは上下方向の角度である。また、ピッチング、ローリング運動も含めて推定する場合は６次元（グローバル座標系におけるｘ， ｙ， ｚ， θ，ι， κ）となる。ιはピッチング量、κはローリング量である。

次に、自己位置推定手段本体６Ｃの処理を、図４を参照して説明する。
先ずステップＳ４１０では、今までの自己位置推定結果に基づき、現在の撮像装置１０位置から見た場合、環境認識マップに保存されている特徴点Ｔが画像上のどの位置に見えるかを算出する。これは環境認識マップ内で、以前行った撮像装置１０位置と環境認識マップ内の特徴点Ｔとの、相対角度と相対距離から計算を行う。
次にステップＳ４２０では、撮像装置１０から現在の画像を取得する。
次にステップＳ４３０では、ステップＳ４１０で画像内での位置を推定した特徴点Ｔが、現在の画像内のどこに存在するのか探索を行う。この探索もステップＳ３３０にて説明した一般的なトラッキング手法を採用すれば良い。

次に、ステップＳ４４０では、実際に特徴点Ｔが存在した画面上の場所と、ステップＳ４１０で推定した特徴点Ｔ推定値の画面上の位置との誤差が最小化するように、撮像装置１０の位置の最適化を行う。最適化手法には、さまざまな手法が存在するが、ロバスト推定であるＭ−ｅｓｔｉｍａｔｏｒ や ＲＡＮＳＡＣなどの手法を最小すればよい。そして、ここで得られた撮像装置１０の位置を、現在の自己位置推定値とする。

また、移動体制御手段６Ｄは、移動体１を自律的に走行させるための制御ルールを備え、状況に応じた適切な移動体１の行動指令を駆動制御部５に出力する。例えば、ある空間を一定の間隔をあけて往復する制御ルール、もしくはあらかじめ規定したルートを自己位置推定情報を利用しながら進行するなどの制御ルールを例示出来る。ただし、これらの制御ルールは、権利範囲を限定するものではない。使用目的などによって制御ルールを設定すればよい。

光軸方向制御手段６Ｅは、図７に示すように、光軸保持手段６Ｅａと、光軸復帰手段６Ｅｂを備える。
光軸保持手段６Ｅａは、移動体制御手段６Ｄからの信号に基づき、移動体１がその場回転していると判定すると、撮像装置１０の光軸の向きがグローバル座標系において同一方向を保持するように、移動体１の移動に伴い回転駆動装置１１を駆動制御する。すなわち、移動体１がその場回転を実施している場合は、移動体１がその場回転を実施している際に、その場回転開始時にグローバル座標系における撮像装置１０が向いていた方向に光軸方向Ｄｒを保つように制御を実施する。その場回転か否かは、回転半径が環境移動量未満か否かで判定する。

光軸保持手段６Ｅａの処理は、この例えば、Ｐ制御により実施する。すなわち、その場回転実施直前のグローバル座標系における撮像装置１０の光軸方向Ｄｒをθ１とし、現在の光軸方向Ｄｒをθ２とした場合、角度調節のための出力を−ａ（θ２−θ１）と設定することで制御を実施する。ａは予め設定したゲインである。ゲインａは固定値でも良いし、移動体１の移動速度などに比例した値でも良い。

これによって、移動体１がその場回転して、移動体１の進行方向Ｄｍの向きが変更されても、図８に示すように、光軸方向Ｄｒを、その場回転する前の方向に保持するように制御する。図８中では、左側方向がその場会回転する直前の進行方向Ｄｍとする。
光軸保持手段６Ｅａの制御は、光軸方向Ｄｒを一定に固定するように保持する制御に限定しない。図９に示すように、環境認識マップ内で既に位置が推定された特徴点Ｔが存在する場合は、少なくとも２点以上（平面移動に限定した場合）の特徴点Ｔが写っている範囲内で光軸方向Ｄｒを回転変位させても、自己位置推定を継続することが可能である。したがって、光軸保持手段６Ｅａによる光軸方向Ｄｒの制御を次のように制御しても良い。すなわち、画像上に、少なくとも２点以上の特徴点Ｔが画角内に入る光軸方向Ｄｒを算出し、その方向を新たにθ３とする。そして、角度調節のための出力を−ａ（θ２−θ３）と設定することで制御を実施する。

なお、光軸保持手段６Ｅａの制御が作動している場合には、移動体１の移動量が無いか小さい。従って、地図の更新は実行しない。
また、光軸復帰手段６Ｅｂは、直進運動を検出すると、光軸の方向が初期方向（移動体１の進行方向Ｄｍ）からずれているか否かを判定する。初期方向からずれていると判定すると、初期方向に対する回転角度差が小さくなる方向に回転変位させる。

ここでは、地図を更新しながら段階的に撮像装置１０の光軸方向Ｄｒを正面に向けることとする。光軸復帰手段６Ｅｂは、環境認識マップ内の特徴点Ｔが少なくとも２点以上（平面移動の場合）が画像に写っている範囲で撮像装置１０の光軸方向Ｄｒを移動体１の正面方向に向けて回転させる。この状態で、移動体１が進行方向Ｄｍに進めば、これまで地図が存在しなかった部分についても地図を更新していくことが可能となる。

（動作・作用）
次に、移動体１の移動に伴うマッピング及び自己位置推定の処理部分について、図１０を参照して説明する。
撮像装置１０から現在の画像を取得する（ステップＳ５１０）。
次に、現在利用できるマップが存在することを確認する（ステップＳ５２０）。まだマップが存在しない場合には、最初の画像を取得してから、移動体１が環境移動量以上の距離を移動したか判定する。（ステップＳ５３０）。環境移動量以上の距離を移動していない場合には、撮像装置１０から現在の画像を取得する（ステップＳ５１０）。
ここで、環境移動量以上の距離を移動したか否かを判定しているのは、マップを初期化する際に行うＭｏｔｉｏｎ Ｓｔｅｒｅｏによる周辺物体の距離計測を行う際の精度を一定以上に保つためである。環境移動量以上の距離を移動した否かの判定は、移動状態取得手段６Ａの取得した直進移動量によって判定する。

マップが存在しない状態で、移動体１が環境移動量以上の距離を移動したと判定すると、環境認識手段６Ｂにおいて、マップの作成を開始する（ステップＳ５４０）。すなわち、環境認識手段６Ｂは、環境認識マップの初期化を行う。ここで環境認識手段６Ｂは、Ｍｏｔｉｏｎ Ｓｔｅｒｅｏ方式を実施し、最初に撮像を行った位置をグローバル座標の中心（原点）として、移動に伴う画像上での特徴点Ｔの変化から、移動体１の周辺物体の３次元実空間での位置を推定する。ここで計測されるのは移動体１と周辺物体の相対的な位置であるが、最初に画像を取得した場所をグローバル座標系の中心とし、初期位置からの移動と、相対位置を足し合わせた位置を対象物体のグローバル座標位置として求めることが出来る。

一方、マップが存在している場合には（ステップＳ５２０）、環境認識マップの初期化を行った位置、または最後にマップを更新した位置から環境移動量の距離を移動したことを判定する（ステップＳ５５０）。なお、距離計測精度を所定以上に保つために、初期化されたマップ内での周辺物体との距離が大きければ、それに応じて環境移動量を増やすなどの処理を実施しても良い。
そして、環境移動量以上の距離を移動したと判定した場合には、環境認識手段６Ｂにおいて、環境認識による環境認識マップの更新を実施する。
また、所定サンプリング周期毎に、自己位置推定手段本体６Ｃにおいて、既に作成した環境認識マップ上での移動体１位置を推定する（ステップＳ５７０）。

また、移動体１がその場回転を実施しているか判定する（ステップＳ５８０）。例えば、単位時間当たりの移動体１の移動量ｄ１＝√（ｘ２ ＋ ｙ２）が一定値以下であり、且つ単位時間当たりの移動体１回転量θが一定値以上となった場合に、その場回転と判定する。これは、上述のように回転半径が環境移動量未満か否かでその場回転か否かと判定することを同義である。
移動体１の自己位置の移動量の判定は、例えば自己位置推定値を用いる。移動体１の移動状態判定手段による判定や移動体制御手段６Ｄの制御指令値に基づき判定しても良い。
ただし、自己位置推定手段の結果を用いて、その場回転か否かを判定することで構成を単純化できる。

移動体１がその場回転を実施していた場合は、移動体１がその場回転を実施していた際に、その場回転開始時に撮像装置１０が向いていた方向に撮像装置１０の光軸を保つように制御を実施する（ステップＳ５９０）。環境認識マップ内において、すでに位置が推定された点が存在する場合は、画像上に、少なくとも２点以上（平面移動に限定した場合）の点が写っている範囲内で光軸方向Ｄｒを回転する位置に制御するようにしても良い。制御処理量としては、一定の方向に固定する制御の方が簡易である。また、画像上に、少なくとも２点以上（平面移動に限定した場合）の点が写っている範囲内で光軸方向Ｄｒを回転する位置に制御すると、光軸を初期方向に復帰させる際の時間を短縮可能である。

一方、その場回転の運動をしていないと判定した場合（ステップＳ５８０）は、直進成分を含む移動の判定を行う（ステップＳ５１０）。これは、移動体１の単位時間当たりの移動量ｄ２＝√（ｘ２＋ｙ２）がある所定値以上であるときに直進運動と判定する。若しくは回転半径が環境移動量以上の場合に直進運動と判定する。ここで、自己位置推定手段の結果を用いて、その場回転か否かを判定することで構成を単純化できる。

直進運動していると判定したときは、光軸方向Ｄｒを移動体１の進行方向Ｄｍに向けるよう制御を実施する（ステップＳ５１１）。すなわち、撮像装置１０の光軸方向Ｄｒがすでに移動体１の進行方向Ｄｍを向いている場合は何もしない。これに対して、回転運動直後などで、移動体１の進行方向Ｄｍと撮像装置１０の光軸方向Ｄｒとが一致していない場合は、光軸復帰手段６Ｅｂにより撮像装置１０の光軸方向Ｄｒと移動体１の進行方向Ｄｍが一致する方向に調整する。

このとき、移動体１の進行方向Ｄｍに位置する環境について環境認識マップが無い状態で、光軸方向Ｄｒを移動体１の進行方向Ｄｍに向けてしまうと、自己位置推定を行うための基準が無いため、自己位置推定を継続することができない。これに対応して、地図を更新しながら段階的に撮像装置１０の光軸方向Ｄｒを正面に向けることとする。環境認識マップ内の特徴点Ｔが少なくとも２点以上（平面移動の場合）撮像装置１０が取得する画像内に写っている範囲で撮像装置１０を正面方向に回転させる。

この状態で、移動体１が進行方向Ｄｍに進めば、これまで地図が存在しなかった部分についてもステップＳ５６０の処理において、地図を更新していくことが可能となる。ここでＳ５６０の処理で地図が更新され、移動体１の進行方向Ｄｍと、現在の撮像装置１０の光軸方向Ｄｒとの間に環境認識マップの特徴点Ｔは位置情報が追加されたら、さらに、少なくとも２点以上の点が画角内に写る範囲で、撮像装置１０の光軸方向Ｄｒを移動体１の進行方向Ｄｍにむける。これを継続することで、最終的には、撮像装置１０の光軸方向Ｄｒを移動体１の進行方向Ｄｍと一致させることが可能となる。

以上のように、本実施形態では、一つの撮像装置１０によるＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）方式を採用する。また、対象までの距離はＭｏｔｉｏｎ Ｓｔｅｒｅｏ技術をベースとする。そのため、“その場回転”のように回転半径が小さい移動の場合は、対象までの視差が発生しないため地図の更新ができず自己位置推定を継続することができなくなることがある（ロストする）。これに対し、本実施形態では、上述のように移動体１がその場回転する場合には、撮像方向を調整することで、自己位置推定を継続的に実施できるようになる。

またその場回転が終了し、移動体１に成分を含む移動が発生すると、撮像装置１０の光軸方向Ｄｒを初期方向に復帰するように制御する。
ここで、回転駆動装置１１、光軸方向制御手段６Ｅ（光軸保持手段６Ｅａ、光軸復帰手段６Ｅｂ）は、撮像方向制御手段を構成する。ステップＳ３２０は特徴点検出手段を構成する。環境認識手段６Ｂ及び自己位置推定手段本体６Ｃは、自己位置推定手段を構成する。

（本実施形態の効果）
（１） 撮像装置１０は、移動体１に搭載され且つ移動体周囲を撮像して画像を取得する。特徴点検出手段は、撮像装置が取得した画像に存在する特徴点を検出する。移動状態取得手段６Ａは、上記移動体の移動状態を取得する。自己位置推定手段は、移動体の移動に伴う画像上での上記特徴点の変化から移動体周囲の物体の位置を検出することで、移動体周囲の物体に対する自己位置を推定する。撮像方向制御手段は、移動体がその場回転していない場合には上記撮像方向を、移動体上の座標系において予め定められた所定の方向である初期方向に制御し、移動体がその場回転している場合には上記撮像方向を、移動体がその場回転する前に取得した画像に存在した特徴点の少なくとも一部を撮像可能な向きに制御する。

移動体１の移動状態（運動）がその場回転の場合には、特徴点Ｔで特定する対象物までの視差が発生しないか発生する視差が小さい。これに対し、本発明によれば、移動体１がその場回転の場合には、移動体１がその場回転する前に取得した画像中に位置する特徴点Ｔが存在する方向、つまり環境認識が実施された方向に画像の撮像方向を保持する。これによって、環境認識を更新できない移動状態であっても、特徴点をロストすることなく、自己位置推定を継続することができる。
すなわち、周囲環境の物体を特定するための特徴点Ｔをロストする可能性を低減して、自己位置推定を継続的に実施可能となる。

（２）撮像方向制御手段は、移動体がその場回転している場合には上記撮像方向を、グローバル座標系において、移動体がその場回転する前の方向に固定されるように制御する。
すなわち、光軸方向Ｄｒを一定方向に固定するように制御する。
この結果、撮像装置１０の光軸方向Ｄｒを制御するための計算コストを一定に抑えることが可能になる。

（３）上記撮像方向制御手段は、移動体がその場回転した後、移動体の直進運動を検知すると、移動体に対する撮像方向を上記初期方向に制御する。
例えば、撮像方向制御手段は、上記撮像方向が初期方向と異なる場合に、移動体１の直進運動を検知すると、移動体１に対する撮像方向を初期方向に変化させる。
これによって、その場回転に対応するために撮像方向が移動体１に対する初期方向がずれても、自己位置推定を継続しながら、光軸方向Ｄｒを進行方向Ｄｍに向けることができる。
なお、光軸方向Ｄｒの初期方向は、移動体１の進行方向Ｄｍと同方向に向けておくことが好ましい。移動体１の進行方向Ｄｍの情報を一番取得したいからである。
（４）撮像方向制御手段は、取得している画像上の特徴点Ｔの変化に基づき、移動体１の直進運動を検知する。
移動体１に直進成分を伴う移動が発生したことを撮像装置１０による自己位置推定を用いて判定することで、非常にシンプルな構成で機能を実現することができる。

１ 移動体
６ 制御コントローラ
６Ａ 移動状態取得手段
６Ｂ 環境認識手段（自己位置推定手段）
６Ｃ 自己位置推定手段本体（自己位置推定手段）
６Ｄ 移動体制御手段
６Ｅ 光軸方向制御手段
６Ｅａ 光軸保持手段（撮像方向制御手段）
６Ｅｂ 光軸復帰手段（撮像方向制御手段）
７ 回転数センサ
１０ 撮像装置
１１ 回転駆動装置
Ｄｍ 進行方向
Ｄｒ 光軸方向（撮像方向）
Ｔ 特徴点

Claims (5)

1. 移動体の自己位置を推定する自己位置推定装置であって、
   移動体に搭載され且つ移動体周囲を撮像して画像を取得する撮像装置と、
   移動体に対する撮像装置の撮像方向を制御する撮像方向制御手段と、
   撮像装置が取得した画像に存在する特徴点を検出する特徴点検出手段と、
   上記移動体の移動状態を取得する移動状態取得手段と、
   移動体の移動に伴う画像上での上記特徴点の変化から移動体周囲の物体の位置を検出することで、移動体周囲の物体に対する自己位置を推定する自己位置推定手段と、を備え、
   上記撮像方向制御手段は、移動体がその場回転していない場合には上記撮像方向を、移動体上の座標系において予め定められた所定の方向である初期方向に制御し、移動体がその場回転している場合には上記撮像方向を、移動体がその場回転する前に取得した画像に存在した特徴点の少なくとも一部を撮像可能な向きに制御することを特徴とする自己位置推定装置。
2. 撮像方向制御手段は、移動体がその場回転している場合には上記撮像方向を、グローバル座標系において、移動体がその場回転する前の方向に固定されるように制御することを特徴とする請求項１に記載した自己位置推定装置。
3. 上記撮像方向制御手段は、移動体がその場回転した後、移動体の直進運動を検知すると、移動体に対する撮像方向を上記初期方向に制御することを特徴とする請求項１または２のいずれかに記載の自己位置推定装置。
4. 撮像方向制御手段は、取得している画像上の特徴点の変化に基づき、移動体の直進運動を検知することを特徴とする請求項３に記載した自己位置推定装置。
5. 移動体に搭載した撮像装置により撮像した画像に存在する特徴点を検出し、移動体の移動に伴う画像上での上記特徴点の変化から、移動体周囲の物体の位置を検出することにより移動体周囲の物体に対する自己位置を推定し、
   上記移動体がその場回転していない場合には上記撮像装置の撮像方向を、移動体上の座標系において予め定められた所定の方向である初期方向とし、上記移動体がその場回転している場合には上記撮像装置の撮像方向を、移動体がその場回転する前に取得した画像に存在した特徴点の少なくとも一部を撮像可能な向きとすることを特徴とする自己位置推定方法。

Images