JP5370122B2

JP5370122B2 - 移動体位置推定装置及び移動体位置推定方法

Info

Publication number: JP5370122B2
Application number: JP2009286760A
Authority: JP
Inventors: 彬陳
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 2009-12-17
Filing date: 2009-12-17
Publication date: 2013-12-18
Anticipated expiration: 2029-12-17
Also published as: JP2011128020A

Description

本件は、移動体位置推定装置及び移動体位置推定方法に関する。

実空間中で動作する自律移動体（例えば、移動ロボット、飛行体など）は、ターゲット（例えば、自動充電するためのステーションや、荷物搬送時における搬送目的場所など）との連結動作を行う必要がある場合がある。この場合、自律移動体をターゲットに対して位置・姿勢合わせする作業が、必要不可欠となる。このため、従来は、自律移動体に搭載したビジョン（ステレオカメラなど）や測距センサ（レーザレンジファインダ：ＬＲＦなど）を利用して、ターゲットの発見やターゲットの位置測定などを行っていた。

ビジョンの場合、画像を使うことから、ターゲットの位置認識（トラッキング）は高精度で行うことができる。しかるに、ビジョンの場合、画像の解像度によってはターゲットの位置姿勢、特に姿勢の同定精度が低くなり、ドッキング精度（成功率）が低下するおそれがある。また、測距センサを用いたターゲットのトラッキングでは、ターゲットの誤追跡や、ターゲットの見失いが生じやすい。このため、測距センサを用いたトラッキングを行う場合、物理的に特殊な形状のターゲットを用いる必要があったり、トラッキング開始時の初期位置姿勢が制限されたりするおそれがある。

これに対し、特許文献１〜３には、複数のセンサを用いたロボット等の位置制御に関する技術が開示されている。特許文献１には、カメラで撮影された画像データとレーザレンジファインダにより取得された距離データとを用いた位置誤差検出を行い、補正を行う技術が開示されている。特許文献２には、前方の障害物をカメラと超音波センサで検出し、当該検出結果から、障害物の位置情報を取得する技術が開示されている。特許文献３には、移動ロボットの制御において、カメラの画像情報と、超音波による距離情報をニューラルネットワークに入力して、ロボット制御情報を作成する技術が開示されている。

特開２００７−３２２１３８号公報特開２０００−１２３２９８号公報特開平６−１２４１２０号公報

上記特許文献１〜３では、ビジョンや測距センサを単独で用いる場合と比較して、高精度なロボットの位置・姿勢制御が可能である。しかるに、今後、自律移動体の技術がより一層進歩すると、これに伴って、位置・姿勢制御に対しても、より一層の精度向上が要求されると考えられる。

一方、最近では、自律移動体の位置・姿勢制御に、パーティクルフィルタと呼ばれる、物体の検出と追跡を同時に行うアルゴリズムが用いられている。このパーティクルフィルタを用いた位置・姿勢制御は、計算量が少ないため、リアルタイム処理が可能であり、現段階でも比較的高い制御精度を得ることができている。しかしながら、このようなパーティクルフィルタを用いた制御においても、今後、より高精度な制御が要求されると考えられる。

そこで本件は上記の課題に鑑みてなされたものであり、移動体がターゲットに近づく際の、移動体の高精度な位置推定を行うことが可能な移動体位置推定装置及び移動体位置推定方法を提供することを目的とする。

本明細書に記載の移動体位置推定装置は、移動体に設けられ、当該移動体周辺の物体との距離を測定する第１測定部と、前記移動体に設けられ、当該移動体の移動目標であるターゲットの画像を取得するとともに、当該画像からターゲットの位置及び姿勢を測定して前記移動体の位置座標を算出する第２測定部と、前記移動体の状態遷移に関する状態遷移情報を取得する状態取得部と、前記第１測定部が測定する距離情報、前記第２測定部が測定する画像位置座標情報、及び前記状態取得部が取得する状態遷移情報から、前記移動体の位置座標を推定するパーティクルフィルタ処理部と、を備え、前記パーティクルフィルタ処理部は、前記移動体が前記ターゲットに近づく前に、前記第２測定部が算出する前記移動体の画像位置座標に対して分散配置させた複数のサンプル座標をランダムに設定するサンプル事前設定部と、前記サンプル事前設定部による複数のサンプル設定後、前記移動体が前記ターゲットに近づいている際に、前記複数のサンプル座標を前記第２測定部にて再取得された前記ターゲットの画像に基づく前記移動体の画像位置座標を用いて前記複数のサンプル座標の適合性を評価するサンプル評価部と、前記サンプル評価部による評価結果に基づいて複数のサンプル座標を再設定するリサンプリング部と、前記リサンプリング部による前記複数のサンプルの再設定後に、前記第１測定部が再測定する距離情報及び前記第２測定部が再測定する画像位置座標情報の少なくとも一方に基づいて、前記再設定後の複数のサンプル座標の修正処理を行う第１修正部と、前記移動体が前記ターゲットに近づいている際に、前記状態取得部が取得する前記状態遷移情報に基づいて、前記第１修正部による修正処理後の複数のサンプル座標を再修正し、前記再修正後の複数のサンプル座標を前記サンプル評価部に引渡す第２修正部と、を有している。

本明細書に記載の移動体位置推定方法は、移動体の位置を推定するために用いる分散配置された複数のサンプルの座標を、前記移動体に設けられた距離測定部により測定された前記移動体と前記ターゲットとの距離情報、及びカメラにより取得されたターゲットの画像から測定される前記移動体の画像位置座標情報、の少なくとも一方を用いて修正する第１修正工程と、前記移動体がターゲットに近づいている際に、前記移動体に設けられた状態取得部が取得する前記移動体の状態遷移情報に基づいて、前記第１修正工程で修正された後のサンプル座標を再修正する第２修正工程と、をパーティクルフィルタ処理部が実行する移動体位置推定方法である。

本明細書に記載の移動体位置推定装置及び移動体位置推定方法は、移動体がターゲットに近づく際の、移動体の高精度な位置推定を行うことができるという効果を奏する。

一実施形態に係るロボットを概略的に示す図である。ロボット及びロボットが配置される空間を上方から見た状態を示す図である。ロボットの制御系を示すブロック図である。ロボットがターゲットを検出して、アプローチを開始するまでの処理を説明するためのフローチャートである。図５（ａ）〜図５（ｃ）は、図４の処理の補足説明図である。パーティクルフィルタ処理部の処理の流れを模式的に示す図である。図７（ａ）は、カメラ１８（ステレオビジョン）の左右カメラで撮影した画像の一例を示す図であり、図７（ｂ）は、ターゲットの３次元計測の結果を投影した床面グリッドマップを示す図である。図８（ａ）、図８（ｂ）は、第１修正部によるサンプルの修正方法を説明するための図である。図９（ａ）〜図９（ｄ）は、ロボットがターゲットに近づくときに行われる処理の具体例及び当該処理による効果について説明するための図である。比較例における図６に対応する図である。比較例における図９に対応する図である。

以下、移動体位置推定装置及び移動体位置推定方法の一実施形態について、図１〜図１１に基づいて詳細に説明する。図１には、移動体位置推定装置としての位置推定部を備えるロボット１００が模式的に示されている。また、図２は、ロボット１００及びロボット１００が配置される空間を上方から見た状態を示している。ロボット１００は、図２に示す部屋などの空間１０００内を移動可能である。また、ロボット１００は、内蔵する充電池の充電量が少なくなったときには、図２に破線にて示すように、充電ステーション（以下、「ターゲット」とも呼ぶ）１０２に戻って、充電を行う機能を有している。

ロボット１００は、図１に示すように、頭部１０、首部１２、胴体部１４、及び移動機構１６を備える。頭部１０には第２測定部としてのカメラ１８が設けられている。カメラ１８は、複数の撮像部を含むステレオビジョンであり、周囲の障害物などの物体の画像を取得し、それらの位置や姿勢を測定するとともに、ターゲット１０２の画像を取得し、ターゲット１０２の位置や姿勢を測定して、ロボット１００の位置座標を算出する。首部１２は、頭部１０の向き（角度）を調整することで、カメラ１８の撮影方向を調整する。胴体部１４は、第１測定部としての測距センサ２０を有する。測距センサ２０は、レーザレンジファインダ（ＬＲＦ）などのセンサであり、周囲の物体との距離を測定し、レンジデータを取得する。なお、測距センサ２０による距離の測定精度は、カメラ１８による測定精度に比べて一般的に高い。移動機構１６は、車輪等を含み、ロボット１００の移動動作を実行する。

図３は、ロボット１００の制御系２００を示すブロック図である。図３に示すように、ロボット１００の制御系２００は、頭部回転系５２と、移動系５４と、移動体位置推定装置としての位置推定部５０と、を有する。

頭部回転系５２は、頭部角度算出部６２と、頭部制御部６４とを有する。頭部角度算出部６２は、位置推定部５０（相対位置・姿勢算出部３０）により算出されるロボット１００とターゲット１０２との相対位置及び姿勢に基づいて、頭部１０（カメラ１８）の目標角度を算出する。この目標角度は、カメラ１８の視野内からターゲット１０２が外れないような角度である。目標角度の算出結果は、頭部角度算出部６２から頭部制御部６４に送信される。頭部制御部６４は、頭部角度算出部６２での算出結果に基づいて、頭部１０（カメラ１８）の向きを制御する。

移動系５４は、移動制御部６６と、前述した移動機構１６（図１参照）と、を有する。移動制御部６６は、ロボット１００の移動を統括的に制御するものである。移動制御部６６は、例えば、ロボット１００がターゲット１０２に近づき始めた際には、相対位置・姿勢算出部３０により算出されるロボット１００とターゲット１０２との相対位置・姿勢に基づいて、ロボット１００の移動量を算出する。そして、移動制御部６６は、当該算出結果に基づいて移動機構１６を制御する。

位置推定部５０は、前述したカメラ１８及び測距センサ２０の他、状態取得部としての移動量検出部２２、記憶部２４、ターゲット検出部２６、パーティクルフィルタ処理部２８、及び相対位置・姿勢算出部３０を有する。移動量検出部２２は、ロボット１００の状態遷移に関する状態遷移情報を取得する。具体的には、移動量検出部２２は、移動機構１６の車輪の回転速度を計測するエンコーダ等を含み、当該車輪の回転速度（又は回転量）から、ロボット１００の走行速度と角速度を求め、ロボット１００の移動量を状態遷移情報として取得する。なお、移動量検出部２２としては、ロボット１００の加速度を検出する加速度センサを含んでいても良い。

記憶部２４は、ターゲット形状記憶部６８と、ターゲットパターン記憶部７０と、を有する。ターゲット形状記憶部６８は、ロボット１００のターゲット（例えば充電ステーションなど）の形状を記憶しているデータベースである。ターゲットパターン記憶部７０は、ターゲットに設けられたマークや、ターゲット表面の模様などのパターンを記憶しているデータベースである。

ターゲット検出部２６は、測距センサ２０により計測されたロボット１００と周辺物体との距離と、カメラ１８により計測されたロボットの位置座標とから、ターゲット１０２の位置を検出する。

パーティクルフィルタ処理部２８は、ターゲット検出部２６の検出結果を用いて、ロボット１００の位置を推定する。パーティクルフィルタ処理部２８は、サンプル事前設定部７２と、サンプル評価部７４と、リサンプリング部７６と、第１修正部７８と、第２修正部８０と、を有する。なお、パーティクルフィルタ処理部２８の構成各部の処理内容については、後述する。

次に、図４のフローチャート及び図５に基づいて、本実施形態のロボット１００が、ターゲット１０２を検出し、アプローチを開始する（近づき始める）までの方法について説明する。

まず、図４のステップＳ１０において、測距センサ２０が、周囲のレンジデータを取得する。なお、測距センサ２０のビームは、ターゲット１０２の表面にも当たっているので、ターゲット１０２のレンジデータも取得することができる。次いで、ステップＳ１２では、相対位置・姿勢算出部３０が、測距センサ２０のビームが当たったすべての点を、ターゲット１０２の中心と仮定して、ターゲット１０２の位置姿勢の仮説群を生成する。図５（ａ）には、生成された仮説群が模式的に示されている。

次いで、ステップＳ１４では、相対位置・姿勢算出部３０が、ステップＳ１２で生成された仮説群と、予め用意されているターゲット１０２の形状モデルとを形状マッチングして、各仮説の適切性を評価する。ここで、ターゲット１０２の形状モデルは、ターゲット形状記憶部６８に格納されている形状モデルである。

次いで、ステップＳ１６では、相対位置・姿勢算出部３０が、仮説の番号を示すパラメータｎを１に設定する。次いで、ステップＳ１８では、相対位置・姿勢算出部３０が、ｎ番目（ここでは１番目）の仮説の適切性が所定の閾値を超えているか否かを判断する。ここでの判断が肯定された場合には、相対位置・姿勢算出部３０は、ステップＳ２０において、ｎ番目（１番目）の仮説を残して、ステップＳ２４に移行する。一方、ステップＳ１８の判断が否定された場合には、相対位置・姿勢算出部３０は、ステップＳ２２においてｎ番目（１番目）の仮説を削除して、ステップＳ２４に移行する。

ステップＳ２４では、相対位置・姿勢算出部３０が、ｎ＝Ｎ（Ｎは、仮説群に含まれる仮説の総数）であるか否かを判断する。ここでの判断が否定された場合にはステップＳ２６に移行して、ｎを１インクリメント（ｎ←ｎ＋１）した後、ステップＳ１８に戻る。以降は、ステップＳ２４の判断が肯定されるまで上記処理・判断を繰り返す。そして、ステップＳ２４の判断が肯定された段階で、ステップＳ２８に移行する。図５（ｂ）には、これら処理・判断を繰り返した結果、削除されずに残った仮説群が模式的に示されている。

次いで、図４のステップＳ２８では、頭部角度算出部６２が、残った仮説が含まれる領域に基づいて、カメラ１８のパン・チルト角度を決定する。そして、ステップＳ３０では、頭部制御部６４が、頭部角度算出部６２にて算出されたパン・チルト角度に基づいて、頭部１０（カメラ１８）の角度を制御し、カメラ１８が撮影を実行する。

次いで、ステップＳ３２では、相対位置・姿勢算出部３０が、残った仮説群をカメラ１８により撮影した撮影画像に投影して、エッジ強さや向きを予測する。次いで、ステップＳ３４では、相対位置・姿勢算出部３０が、予測したエッジ強さや向きとターゲット１０２の形状モデルとを比較して、各仮説に尤度を付ける。図５（ｃ）では、ステップＳ３４の結果、尤度が高いと判断された仮説を濃く表示している。

次いで、図４のステップＳ３６では、相対位置・姿勢算出部３０が、尤度が最大値とされた仮説を抽出することで、ターゲット１０２の位置・姿勢を決定する。そして、ステップＳ３８では、移動制御部６６が、相対位置・姿勢算出部３０により算出されたターゲット１０２の位置・姿勢に基づいて、移動機構１６を制御し、ロボット１００のターゲット１０２に向けたアプローチを開始する。

このように、本実施形態では、ロボット１００は、測距センサ２０の測定結果から求められる仮説群、カメラ１８による撮影画像、事前に用意したターゲット１０２の形状モデル（物理的モデル）を用いて、３次元におけるターゲット１０２との相対位置・姿勢を推定する。したがって、ロボット１００は、ターゲット１０２との相対位置・姿勢を精度良く推定することが可能となっている。

次に、ロボット１００が上述のようにターゲット１０２に向けたアプローチを開始した後における、ターゲット１０２の追跡方法の原理について、図６に基づいて説明する。図６は、パーティクルフィルタ処理部２８の処理の流れを模式的に示す図である。

パーティクルフィルタ処理部２８では、サンプル事前設定部７２が、ターゲット検出部２６の検出結果に基づき、推定したいロボット１００の状態を所定の確率分布に従って分散配置して、一定数のサンプルをランダムに設定する。

具体的には、以下のようにしてサンプルを設定する。
（ａ）ターゲット検出部２６が、ロボット１００の状態として、座標データを算出する。ここで算出された座標データを、（ｘ、ｙ、θ）＝（１００、１００、５０）とする。なお、ｘ、ｙはターゲット１０２とロボット１００間の距離情報を意味し、θはターゲット１０２と移動体との間の姿勢情報を意味する。
（ｂ）パーティクルフィルタ処理部２８が、ターゲット検出部２６が算出した座標データを所定の確率分布に従って離散化して、一定数のサンプルをランダムに設定する。ここで、所定の確率分布としては、一様分布や正規分布などを用いることができる。
例えば、一様分布を採用した場合、ターゲット検出部２６が算出した座標（ｘ、ｙ、θ）＝（１００、１００、５０）を中心とすると、サンプル１を中心座標からそれぞれ＋２ずらし、サンプル２を中心座標からそれぞれ＋１ずらし、サンプル３を中心座標からそれぞれ±０ずらし、サンプル４を中心座標からそれぞれ−１ずらし、サンプル５を中心座標からそれぞれ−２ずらし、…という処理を行う。この場合、サンプル１の座標が（ｘ、ｙ、θ）＝（１０２、１０２、５２）、サンプル２の座標が（ｘ、ｙ、θ）＝（１０１、１０１、５１）、サンプル３の座標が（ｘ、ｙ、θ）＝（１００、１００、５０）、サンプル４の座標が（ｘ、ｙ、θ）＝（９９、９９、４９）、サンプル５の座標が（ｘ、ｙ、θ）＝（９８、９８、４８）…、となる。パーティクルフィルタ処理部２８は、このような、（ｘ、ｙ、θ）の座標情報を有する複数のサンプルを、乱数を用いてランダムに設定する。なお、図６の（ａ）では、５つのサンプルが設定されているものとする。

次いで、サンプル評価部７４が、図６の（１）に示すように、サンプル事前設定部７２による複数のサンプル設定後、ロボット１００がターゲット１０２に近づいている際に、複数のサンプル座標をカメラ１８にて再取得（再撮影）されたターゲット１０２の画像に基づくロボットの位置座標を用いて、複数のサンプル座標の適合性を評価する。すなわち、サンプル評価部７４は、ターゲット１０２の画像からターゲット１０２の位置及び姿勢を測定して、ロボット１００の位置座標を算出し、当該算出された位置座標に基づいて、サンプルを評価する。この図６の（１）の処理により、各サンプルが格付けされる。格付けの結果として、図６の（ｂ）では、格付けの高いサンプルを大きく表示し、格付けの低いサンプルを小さく表示している。

次いで、図６の（２）に示すように、リサンプリング部７６が、サンプル評価部７４による評価結果に基づいて複数のサンプル座標を再設定する。具体的には、リサンプリング部７６は、図６の（ｃ）に示すように適合性の低いサンプルを消し、高いものをコピーして重み付けを行う。

次いで、図６の（３）に示すように、第１修正部７８が、外部モデルに基づいて、再設定後の複数のサンプル座標の修正処理を行う。当該修正により移動したサンプルが図６の（ｄ）に示されている。

ここで、外部モデルに基づくサンプルの修正方法について、図７、図８に基づいて詳細に説明する。

まず、第１修正部７８は、以下のようにして、カメラ１８を用いた仮計測を実行する。
（Ａ）サンプルの修正直前にカメラ１８により撮影された画像（図７（ａ）に示すようなステレオビジョンの左右カメラで撮影した画像）を取得する。そして、第１修正部７８は取得した画像中のターゲット１０２の形状及び大きさから、ターゲット１０２の位置・姿勢を３次元計測する。次いで、第１修正部７８は、図７（ｂ）に示すように、ターゲット１０２を３次元計測の結果を床面グリッドマップ上に投影する。
（Ｂ）次に、第１修正部７８は、床面グリッドマップ上で図７（ｂ）に示すターゲットの向きＲ、及び中心位置Ｐの位置を計算する。さらに、第１修正部７８は、ロボット１００からターゲット１０２の両端までのなす角度ψを計算する。

以上のようにして仮計測が終了すると、第１修正部７８は、図８（ａ）に示すように、角度ψの範囲内で、測距センサ２０を用いた計測を実際に行い、計測値（レンジデータ）を取得する。そして、第１修正部７８は、計測値を用いた直線当てはめを行うことにより、ターゲット１０２の存在する領域（ターゲットの姿勢及び中心位置Ｐ’）を算出する。これにより、測距センサ２０の計測結果から、ターゲット１０２とロボット１００との間の距離情報及び姿勢情報を示す座標（Ｘ、Ｙ、Θ）を求めることができる。

次いで、第１修正部７８は、測距センサ２０で計測したターゲット１０２の位置・姿勢の座標をカメラ１８で計測したターゲット１０２の位置姿勢に合わせるように、現在のサンプルの状態（座標（ｘ、ｙ、θ）で表されるが、ここでは状態Ｓとする。図８（ｂ）参照）を、新しい状態Ｓ’（座標（ｘ’、ｙ’、θ’））に移動する。そして、第１修正部７８は、新しい状態Ｓ’（座標（ｘ’、ｙ’、θ’））と元の状態Ｓ（座標（ｘ、ｙ、θ））とを平均することで、修正後のサンプルの状態として、図８（ｂ）に示す状態Ｓ” （座標（ｘ”、ｙ”、θ”））を取得する。すなわち、図６の（３）に示すサンプルの修正では、サンプルをＳ（座標（ｘ、ｙ、θ））からＳ” （座標（ｘ”、ｙ”、θ”））に変更するような修正を実行する。

なお、上記においては、カメラ１８を用いた仮計測の結果と、当該仮計測に基づいて測距センサ２０を用いて行った計測の結果と、を用いて、サンプルの修正を行うこととしたが、これに限られるものではない。例えば、カメラ１８の仮計測の結果のみを用いて、サンプルの状態を修正することとしても良い。具体的には、カメラ１８の仮計測結果から求められるターゲット１０２とロボット１００との間の距離情報及び姿勢情報を示す座標（ｘ、ｙ、θ）と、サンプルの中心座標（ｘ、ｙ、θ）とのずれ量を算出し、当該ずれ量分だけ、各サンプルを移動しても良い。また、例えば、測距センサ２０の計測結果のみを用いて、サンプルの状態を修正することとしても良い。具体的には、測距センサ２０の計測結果から求められるターゲット１０２とロボット１００との間の距離情報及び姿勢情報を示す座標（ｘ、ｙ、θ）と、サンプルの中心座標（ｘ、ｙ、θ）とのずれ量を算出し、当該ずれ量分だけ、各サンプルを移動しても良い。

次いで、第２修正部８０は、ロボット１００がターゲット１０２に近づいている際に、図６の（４）に示すように、内部センサの取得結果（ここでは、移動量検出部２２の計測結果（状態遷移情報））に基づいて、第１修正部７８において修正された後のサンプル座標を再修正する（（ｅ）参照）。すなわち、ロボット１００の移動量（実測値）分だけ、サンプルを移動する。なお、この修正は、ダイナミック遷移とも呼ばれている。その後は、（ｅ）のサンプルがサンプル評価部７４に引渡されることにより（ａ）のサンプルとなり（図６の（５））、上記（１）〜（４）の処理が、パーティクルフィルタ処理部２８内において随時繰り返される。

ここで、ロボット１００がターゲット１０２に近づくときに行われる処理の具体例及び当該処理による効果について、図９に基づいて詳細に説明する。

図９（ａ）に示すように、ある時刻ｔにおいて、平面上に、サンプル(Ａ、Ｂ、Ｃ、Ｄ)が生成されたものとする（なお、サンプル（Ａ、Ｂ、Ｃ、Ｄ）は、図６では（ｄ）に示すサンプルに該当するものとする）。このときのサンプル（座標）の平均値（平均期待値とも呼ばれる）をＫとする。そして、この状態から、移動制御部６６が、移動機構１６に対し、ターゲット１０２に向けて距離ｄ₁だけ移動する指令（ｖ）を発行したものとする。

この場合、第２修正部８０は、ロボット１００が実際に移動した距離（Ｖ)を移動量検出部２２で計測し、その計測値（Ｖ)でサンプル（Ａ、Ｂ、Ｃ、Ｄ)の状態を変化させる。図９（ａ）では、この状態を時刻ｔ+１で示している。なお、時刻ｔ＋１のサンプルは、図６では（ｅ）及び（ａ）に示すサンプルに該当する。

次いで、カメラ１８による計測結果に基づいて、サンプル評価部７４が、サンプル(Ａ、Ｂ、Ｃ、Ｄ)を評価する。またリサンプリング部７６は、評価結果に基づいて、リサンプリングを行う。ここでは、サンプルＤの評価が低かったため、サンプルＤが削除されるとともに、サンプルＣの評価が高かったため、サンプルＣがコピーされたものとする。

次いで、第１修正部７８は、測距センサ２０で計測されたロボット１００とターゲット１０２との相対位置・姿勢及びカメラ１８で計測されたロボット１００とターゲット１０２との相対位置・姿勢を用いて、サンプル（Ａ、Ｂ、Ｃ、Ｃ)をサンプル（Ａ’、Ｂ’、Ｃ’、Ｃ’)に修正する（図９（ｂ）参照）。この修正は、図７及び図８を用いて説明した方法で行われる。なお、サンプル（Ａ’、Ｂ’、Ｃ’、Ｃ’)の平均をＫ’とする。この修正により、平均値Ｋ’は平均値Ｋよりもロボット１００の真の位置に近づいたといえる。

次いで、移動制御部６６が、移動機構１６に対し、ターゲット１０２に向けて距離ｄ₂だけ移動するよう指令（ｖ’）を発行する。このアプローチ指令後、第２修正部８０は、ロボット１００が実際に移動した距離（Ｖ')を移動量検出部２２で計測し、この計測値（Ｖ’)でサンプル（Ａ’、Ｂ’、Ｃ’、Ｃ’)の状態を変化させる。図９（ｃ）では、この状態を時刻ｔ+２で示している。

その後も、サンプル評価部７４による評価、リサンプリング部７６によるリサンプリングを経て（リサンプリングにより残ったサンプルを（Ａ’、Ｂ’、Ｃ’、Ｃ’)とする）、第１修正部７８がサンプルを補正する。この補正後のサンプルが図９（ｄ）に示すサンプル（Ａ”、Ｂ”、Ｃ”、Ｃ”)で、サンプルの平均がＫ”であったものとする。

その後、移動制御部６６が、移動機構１６に対し、ターゲット１０２に向けて距離ｄ₃だけ移動するよう指令（ｖ”）を発行する。この場合、距離ｄ₃は、実際のロボット１００とターゲット１０２との距離と近似しているので、図９（ｄ）に示すロボット１００の最終ドッキング位置にロボット１００を移動させることが可能となる。

次に、本実施形態の比較例について説明する。

図１０には、本実施形態の比較例に係るパーティクルフィルタの処理例が示されている。この図１０と図６とを比較すると分かるように、比較例では、サンプルを外部モデルで修正する処理を行っていない。

この比較例の場合においても、図１１に示すように、本実施形態と同様、移動制御部６６から移動機構１６に指令（ｖ、ｖ’、ｖ”）が順次出される。この場合、図１１に示すように、時刻ｔ＋２の段階で、目標距離ｄ₃がロボット１００とターゲット１０２との間の実際の距離Ｄよりも短い可能性がある。この場合、最終ドッキング位置が図１１に示すようにターゲット１０２の手前になってしまう。また、図１１とは逆に、時刻ｔ＋２の段階で、目標距離ｄ₃が、ロボット１００とターゲット１０２との間の距離Ｄよりも長い可能性がある。この場合には、ターゲット１０２に勢い良くぶつかってしまう可能性がある。この点、本実施形態では、目標距離と実際の距離を近似させることができるので、ロボット１００をターゲット１０２に対して高精度に移動することができる。

以上、詳細に説明したように、本実施形態では、パーティクルフィルタ処理部２８が、移動体がターゲットに近づく際にランダムに複数のサンプルを設定するサンプル事前設定部７２と、サンプルをカメラ１８にて取得されたターゲット１０２の画像に基づいて評価するサンプル評価部７４と、当該評価結果に基づいて複数のサンプルを再設定するリサンプリング部７６と、サンプルの再設定後に、測距センサ２０及びカメラ１８による測定結果に基づいてサンプルの修正処理を行う第１修正部７８と、移動量検出部２２の計測結果に基づいて、第１修正部７８による修正処理後のサンプルを更に修正し、サンプル評価部７４に戻す第２修正部８０と、を有している。このように、本実施形態では、第１修正部７８が、外部モデルに基づいてサンプルの修正処理を行うので、移動量検出部２２に計測誤差がある場合であってもロボット１００の移動中にサンプルの修正処理を適宜行うことで、ロボット１００の位置を高精度に推定することができる。これにより、ロボット１００とターゲット１０２との相対位置を高精度に推定することができるので、ターゲット１０２に対するロボット１００の移動を高精度に行うことが可能である。また、本実施形態では、第１修正部７８は、カメラ１８の測定結果のみならず、一般的にカメラ１８よりも計測精度が高い測距センサ２０による測定結果も用いてサンプルの修正を行うので、高精度なサンプル修正が期待できる。

また、本実施形態では、第１修正部７８は、当該第１修正部７８によるサンプル座標の修正処理を行う前に、カメラ１８が再測定したターゲット１０２の位置及び姿勢の情報を用いて測距センサ２０が測定したターゲット１０２までの距離と、位置及び姿勢の情報と、を用いて、前記ターゲットの位置及び姿勢を算出し、カメラ１８で測定されたターゲットの位置及び姿勢と一致するように再設定後のサンプルを移動するとともに、サンプルの移動前の位置（状態）Ｓと移動後の位置（状態）Ｓ’との平均位置Ｓ”までサンプルを移動することで、サンプルの修正を行う。これにより、測距センサ２０とカメラ１８の測定結果のいずれをも考慮した、高精度なサンプルの修正を行うことができる。

なお、上記実施形態では、第１修正部７８が、サンプルの状態ＳとＳ’の平均位置Ｓ”に、サンプルを移動する場合について説明したが、これに限られるものではない。例えば、Ｓ、Ｓ’のいずれかのほうが信頼性が高いような場合には、当該信頼性に応じた重み付け平均により、Ｓ”を決定することとしても良い。

なお、上記実施形態では、第１修正部７８が、測距センサ２０とカメラ１８の測定結果を用いて、サンプルの修正を行う場合について説明したが、これに限られるものではない。測距センサ２０とカメラ１８のいずれか一方の測定結果を用いることとしても良い。

上述した実施形態は本発明の好適な実施の例である。但し、これに限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々変形実施可能である。

１８カメラ（第２測定部）
２０測距センサ（第１測定部）
２２移動量検出部（状態取得部）
２８パーティクルフィルタ処理部
５０位置推定部（移動体位置推定装置）
７２サンプル事前設定部
７４サンプル評価部
７６リサンプリング部
７８第１修正部
８０第２修正部
１００ロボット（移動体）
１０２ターゲット

Claims

移動体に設けられ、当該移動体周辺の物体との距離を測定する第１測定部と、
前記移動体に設けられ、当該移動体の移動目標であるターゲットの画像を取得するとともに、当該画像からターゲットの位置及び姿勢を測定して前記移動体の位置座標を算出する第２測定部と、
前記移動体の状態遷移に関する状態遷移情報を取得する状態取得部と、
前記第１測定部が測定する距離情報、前記第２測定部が測定する画像位置座標情報、及び前記状態取得部が取得する状態遷移情報から、前記移動体の位置座標を推定するパーティクルフィルタ処理部と、を備え、
前記パーティクルフィルタ処理部は、
前記移動体が前記ターゲットに近づく前に、前記第２測定部が算出する前記移動体の画像位置座標に対して分散配置させた複数のサンプル座標をランダムに設定するサンプル事前設定部と、
前記サンプル事前設定部による複数のサンプル設定後、前記移動体が前記ターゲットに近づいている際に、前記複数のサンプル座標を前記第２測定部にて再取得された前記ターゲットの画像に基づく前記移動体の画像位置座標を用いて前記複数のサンプル座標の適合性を評価するサンプル評価部と、
前記サンプル評価部による評価結果に基づいて複数のサンプル座標を再設定するリサンプリング部と、
前記リサンプリング部による前記複数のサンプルの再設定後に、前記第１測定部が再測定する距離情報及び前記第２測定部が再測定する画像位置座標情報の少なくとも一方に基づいて、前記再設定後の複数のサンプル座標の修正処理を行う第１修正部と、
前記移動体が前記ターゲットに近づいている際に、前記状態取得部が取得する前記状態遷移情報に基づいて、前記第１修正部による修正処理後の複数のサンプル座標を再修正し、前記再修正後の複数のサンプル座標を前記サンプル評価部に引渡す第２修正部と、
を有することを特徴とする移動体位置推定装置。
前記第１修正部は、
当該第１修正部による前記サンプル座標の修正処理を行う前に、前記第２測定部が再測定した前記ターゲットの位置及び姿勢の情報を用いて前記第１測定部が測定した前記ターゲットまでの距離と、前記位置及び姿勢の情報と、を用いて、前記ターゲットの位置及び姿勢を算出し、前記第２測定部で測定された前記ターゲットの位置及び姿勢と一致するように前記再設定後のサンプルの座標を修正するとともに、前記再設定後のサンプルの座標の修正前の位置と修正後の位置とに基づいて求められる新たな修正位置に、前記再設定後のサンプル座標を修正することを特徴とする請求項１に記載の移動体位置推定装置。
前記修正位置は、前記再設定後のサンプルの修正前の位置と修正後の位置との平均位置であることを特徴とする請求項２に記載の移動体推定装置。
移動体の位置を推定するために用いる分散配置された複数のサンプルの座標を、前記移動体に設けられた距離測定部により測定された前記移動体と前記ターゲットとの距離情報、及びカメラにより取得されたターゲットの画像から測定される前記移動体の画像位置座標情報、の少なくとも一方を用いて修正する第１修正工程と、
前記移動体がターゲットに近づいている際に、前記移動体に設けられた状態取得部が取得する前記移動体の状態遷移情報に基づいて、前記第１修正工程で修正された後のサンプル座標を再修正する第２修正工程と、をパーティクルフィルタ処理部が実行する移動体位置推定方法。