JP2018001348A - 情報処理装置、情報処理方法、及び、プログラム - Google Patents

Abstract

【課題】 未観測領域の観測に適した経路を出力する。【解決手段】 本発明の情報処理装置は、マニピュレータの第１の目標状態と、地図情報と、マニピュレータの現在状態とを基に、第１の目標状態への第１の経路を生成する経路生成手段と、第１の経路と、地図情報と、現在状態とを基に、第１の経路における未観測領域を抽出し、障害物がない領域から未観測領域の近傍領域を抽出し、近傍領域において未観測領域を観測させる第２の目標状態を生成し、現在状態から第２の目標状態までの第２の経路を生成する未観測領域観測計画手段と、第１の経路又は第２の経路を基にマニピュレータを制御する制御入力演算手段とを含む。【選択図】 図５

Description

本発明は、情報の処理に関し、特に、移動するための経路を算出する情報処理装置、情報処理方法、及び、プログラムに関する。

人間の腕又は手先と同様な機能を備えた腕ロボット、すなわちマニピュレータが、広く利用されている。

マニピュレータの利用において、マニピュレータの動作における経路探査が、実行される（例えば、特許文献１及び２を参照）。

しかし、経路探査において、探査する全ての領域が観測済みとは限らない。

特許文献１及び２に記載の発明は、未観測領域を想定していないため、未観測領域がある場合に動作できないという問題点があった。

そこで、未観測領域の探査を取り扱ったロボットが、提案されている（例えば、特許文献３を参照）。特許文献３に記載の発明は、地図を用いて、ヒューマノイドロボットの歩行のための経路を計画する。より詳細には、特許文献３に記載の発明は、まず、未観測領域をフリースペースと見なして、経路を計画する。そして、経路上に未観測領域があった場合、特許文献３に記載の発明は、ロボットを、その領域の手前まで移動する。そして、特許文献３に記載の発明は、ロボットの視線（ロボットの頭部に搭載したセンサ）を、未観測領域が観測される方向に制御し、距離画像を観測し、地図を更新する。このような動作を基に、特許文献３に記載の発明は、目的地に移動するために必要な未観測領域のみを観測して、効率的な目的地への移動を実現する。

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

しかしながら、特許文献３に記載の発明は、最初に算出した経路に沿って移動し、未観測了領域を観測する。そのため、特許文献３に記載の発明は、未観測領域における観測に適した状態で未観測領域に到達するとは限らない。つまり、特許文献３に記載の発明は、未観測領域の観測に適した経路を出力できないという問題点があった。

本発明の目的は、上記問題点を解決し、未観測領域の観測に適した経路を出力する情報処理装置、情報処理方法、及び、プログラムを提供することにある。

本発明の一形態における情報処理装置は、マニピュレータの第１の目標状態と、地図情報と、マニピュレータの現在状態とを基に、第１の目標状態への第１の経路を生成する経路生成手段と、第１の経路と、地図情報と、現在状態とを基に、第１の経路における未観測領域を抽出し、障害物がない領域から未観測領域の近傍領域を抽出し、近傍領域において未観測領域を観測させる第２の目標状態を生成し、現在状態から第２の目標状態までの第２の経路を生成する未観測領域観測計画手段と、第１の経路又は第２の経路を基にマニピュレータを制御する制御入力演算手段とを含む。

本発明の一形態における情報処理方法は、マニピュレータの第１の目標状態と、地図情報と、マニピュレータの現在状態とを基に、第１の目標状態への第１の経路を生成し、第１の経路と、地図情報と、現在状態とを基に、第１の経路における未観測領域を抽出し、障害物がない領域から未観測領域の近傍領域を抽出し、近傍領域において未観測領域を観測させる第２の目標状態を生成し、現在状態から第２の目標状態までの第２の経路を生成し、第１の経路又は第２の経路を基にマニピュレータを制御する。

本発明の一形態におけるプログラムは、マニピュレータの第１の目標状態と、地図情報と、マニピュレータの現在状態とを基に、第１の目標状態への第１の経路を生成する処理と、第１の経路と、地図情報と、現在状態とを基に、第１の経路における未観測領域を抽出する処理と、障害物がない領域から未観測領域の近傍領域を抽出する処理と、近傍領域において未観測領域を観測させる第２の目標状態を生成する処理と、現在状態から第２の目標状態までの第２の経路を生成する処理と、第１の経路又は第２の経路を基にマニピュレータを制御する処理とをコンピュータに実行させる。

本発明に基づけば、未観測領域の観測に適した経路を出力するとの効果を奏することができる。

図１は、一般的なマニピュレータの構成の一例を示す図である。 図２は、マニピュレータの操作状態を模式的に示した図である。 図３は、第１の実施形態に係る情報処理装置を含む操作システムの構成の一例を示すブロック図である。 図４は、一般的な情報処理装置の構成の一例を示すブロック図である。 図５は、本発明における第１の実施形態に係る情報処理装置の構成の一例を示すブロック図である。 図６は、第１の実施形態に係る経路生成部の動作の一例を示すフローチャートである。 図７は、第１の実施形態に係る未観測領域観測計画部の全体動作の一例を示すフローチャートのである。 図８は、第１の実施形態に係る未観測領域観測計画部における修正目標状態を生成する動作の一例を示すフローチャートである。 図９は、現在状態と、未観測領域における衝突を模式的に示す図である。 図１０は、近傍フリー集合の一例を示す図である。 図１１は、修正目標状態候補の一例を示す図である。 図１２は、修正経路候補の一例を示す図である。 図１３は、修正目標状態候補の評価値を算出する経路と、地図用センサのセンス範囲との一例を示す図である。 図１４は、修正目標状態として選択された修正目標状態候補を示す図である。 図１５は、エンドエフェクタが修正目標状態に到達した状態を示す図である。 図１６は、目標状態に到達したマニピュレータを示す図である。 図１７は、変形例に係る情報処理装置の構成の一例を示すブロック図である。

次に、本発明における実施形態について図面を参照して説明する。

なお、各図面は、本発明の実施形態を説明するためのものである。ただし、本発明は、各図面の記載に限られるわけではない。また、各図面の同様の構成には、同じ番号を付し、その繰り返しの説明を、省略する場合がある。また、以下の説明に用いる図面において、本発明の説明に関係しない部分の構成については、記載を省略し、図示しない場合もある。また、図面中の矢印の方向は、一例を示すものであり、ブロック間の信号の向きを限定するものではない。

本発明における実施形態の理解を容易にするため、図面を参照して、一般的なマニピュレータの構成について説明する。

図１は、一般的なマニピュレータ２１０の構成の一例を示す図である。

図１に示されているマニピュレータ２１０は、基台２１３と、一つ又は複数のリンク２１１と、一つ又は複数の関節部２１２と、エンドエフェクタ２１５とを含む。さらに、マニピュレータ２１０は、地図用センサ２２０を含む。

基台２１３は、マニピュレータ２１０を所定に位置に固定する。さらに、基台２１３は、後ほど説明する情報処理装置１００及び地図生成部２３０を含んでもよい。

リンク２１１は、関節部２１２及びエンドエフェクタ２１５を固定的に結合する構成である。

関節部２１２は、リンク２１１を回転可動に接続する構成である。

なお、関節部２１２が、マニピュレータ２１０の動力源（例えば、モーター）を含んでもよい。あるいは、リンク２１１が動力源（例えば、油圧シリンダ）を含んでもよい。

エンドエフェクタ２１５は、実際の作業（例えば、物品の保持）を実行する構成である。詳細には、エンドエフェクタ２１５は、その構成として、リンク２１１及び関節部２１２に相当する構成を含み、所定の作業を実現している。

地図用センサ２２０は、地図情報に含まれる未観測領域の状態を観測するための検出器である。例えば、地図用センサ２２０は、ＴｏＦ（Time of Flight）方式の深度センサ、又は、奥行き情報を取得できるステレオカメラである。

なお、マニピュレータ２１０は、３次元空間を動作してもよく、２次元平面を動作してもよい。

図１に示されているマニピュレータ２１０は、具体的には、操作装置３０からの制御を基に動作する。

図２は、マニピュレータ２１０の操作状態を模式的に示した図である。

図２に示されているように、マニピュレータ２１０は、所定の通信を介して接続されている操作装置３０からの指示を基に、対象物４０における動作を実行する。

なお、図２において、基台２１３は、一例として、移動可能な台車上に取り付けられている。

＜第１の実施形態＞
次に、図面を参照して、本発明における第１の実施形態に係る情報処理装置１００について説明する。以下の説明は、情報処理装置１００の動作の理解の補助のため、適宜、第１の実施形態に係る情報処理装置１００を含むマニピュレータ２１０を操作するための操作システム１０を参照して説明する。

［構成の説明］
図３は、第１の実施形態に係る情報処理装置１００を含む操作システム１０の構成の一例を示すブロック図である。

図３に示されているように、操作システム１０は、操作装置３０と、マニピュレータ部２０とを含む。

操作装置３０は、入力装置３１０と、出力装置３２０とを含む。

入力装置３１０は、図示しない操作者の操作入力として、マニピュレータ２１０に含まれるエンドエフェクタ２１５の目標状態（例えば、目標となる位置及び姿勢）の入力を受け付ける。そして、入力装置３１０は、受け付けた目標状態をマニピュレータ部２０に送信する。なお、図２に示されているように、入力装置３１０は、この送信に所定の通信路を使用する。

出力装置３２０は、マニピュレータ部２０からマニピュレータ２１０の操作に関連する情報を受信し、図示しない操作者に対して確認可能な情報として出力する。確認可能な情報とは、操作者の視覚、聴覚、又は、触覚を介して取得される情報である。例えば、視覚の場合について説明すると、出力装置３２０は、受信した視覚情報を図示しない表示機器に画像として表示する。あるいは、操作装置３０が操作用のレバーを含む場合、出力装置３２０は、レバーを操作する手などに対する反力として、情報を出力してもよい。

マニピュレータ２１０の操作に関連する情報は、詳細には、図３に示すように、地図情報と、マニピュレータ２１０の状態（例えば、位置及び姿勢）に関する情報とを含む。

なお、マニピュレータ２１０の状態は、取得した時点におけるマニピュレータ２１０の状態である。つまり、マニピュレータ２１０の状態は、その時点における「マニピュレータ２１０の現在状態」である。

マニピュレータ部２０は、マニピュレータ２１０と、地図用センサ２２０と、地図生成部２３０と、第１の実施形態に係る情報処理装置１００とを含む。

マニピュレータ２１０は、情報処理装置１００からの制御に関する情報（以下、「制御入力」と呼ぶ）を基に、各部の位置及び姿勢を変更する。さらに、マニピュレータ２１０は、関節部２１２に関する情報（マニピュレータ関節情報）を情報処理装置１００及び地図生成部２３０に送信する。

マニピュレータ関節情報とは、例えば、マニピュレータ２１０に含まれる関節部２１２の状態（例えば、回転角）に関する情報である。一般的に、マニピュレータ２１０の基台２１３及びリンク２１１は、外形が一定である。また、エンドエフェクタ２１５の外形は、作業に沿って、ある程度変化するが、所定の範囲に含まれる。そのため、情報処理装置１００は、マニピュレータ２１０の関節部２１２の状態を基に、マニピュレータ２１０の各部の位置及び姿勢を算出できる。なお、情報処理装置１００は、初期値として、又は、動作に先立つ処理における受信情報として、算出に必要は情報（例えば、リンク２１１、関節部２１２、及び、エンドエフェクタ２１５の外形）を保持している。

なお、第１の実施形態は、上記の情報を取得できるマニピュレータ２１０であれば、その構成などを制限されない。例えば、マニピュレータ２１０は、既に説明した一般的はマニピュレータ２１０でもよい。あるいは、マニピュレータ２１０は、リンク型に限らず、直行型のロボットでもよい。そのため、マニピュレータ２１０の詳細な説明を省略する。

地図用センサ２２０は、既に説明したように、地図情報の含まれる未観測領域の状態を観測するための検出器である。地図用センサ２２０は、検出した未観測領域の状態（以下、「センサデータ」と呼ぶ）を地図生成部２３０に送信する。なお、観測結果は、少なくとも「障害物がある」及び「障害物がない（フリースペース）」のいずれかを含む。

地図生成部２３０は、マニピュレータ２１０からのマニピュレータ関節情報と、地図用センサ２２０からのセンサデータとを基に、地図情報及びマニピュレータ２１０の現在状態を生成する。そして、地図生成部２３０は、地図情報及びマニピュレータ２１０の現在状態を、情報処理装置１００及び操作装置３０に送信する。

なお、地図生成部２３０は、予め、上記の情報以外の情報として、地図を作成するために必要となる情報を取得又は保持している。

さらに、地図生成部２３０は、地図の生成手法を制限されない。地図生成部２３０は、ＩＣＰ（Iterative Closest Point）等のスキャンマッチング手法を用いてもよい。

また、地図生成部２３０が生成する地図情報の形式は、特に制限されない。例えば、地図生成部２３０は、生成する地図として、３次元占有格子地図（尤度付きボクセルマップ）を生成してもよい。

情報処理装置１００は、操作装置３０から目標状態を受信する。さらに、情報処理装置１００は、マニピュレータ２１０からマニピュレータ関節情報を受信する。さらに、情報処理装置１００は、地図生成部２３０から地図情報及びマニピュレータ２１０の現在状態を受信する。そして、情報処理装置１００は、マニピュレータ２１０の動作を制御するための制御入力を作成し、マニピュレータ２１０に送信する。

次に、第１の実施形態に係る情報処理装置１００を説明するための参考として、マニピュレータ２１０を操作するために一般的に用いられている情報処理装置９００について説明する。情報処理装置９００は、図３に示されている情報処理装置１００と同様の位置において用いられる。

図４は、一般的な情報処理装置９００の構成の一例を示す図である。

情報処理装置９００は、経路生成部９１０と、制御入力演算部９３０とを含む。

経路生成部９１０は、目標状態と、地図情報と、マニピュレータ２１０の現在状態とを基に、所定の手法を用いて、障害物との衝突を回避した経路を生成する。

制御入力演算部９３０は、生成された経路に追従するための制御入力を算出し、マニピュレータ２１０に送信する。さらに、制御入力演算部９３０は、マニピュレータ２１０からのマニピュレータ関節情報を基に制御入力を修正し、マニピュレータ２１０に修正した制御入力を送信する。つまり、制御入力演算部９３０は、フィードバック制御を実現している。

このような一般的な情報処理装置９００が、特許文献３に記載の方法（以下、「第１の一般的方法」と呼ぶ）を用いた場合、既に説明したように、未観測領域の観測に適した経路を出力できないという問題点があった。

また、未観測領域についての処理方法としては、特許文献３に記載の方法以外として、未観測領域を障害物と見立てる方法（以下、「第２の一般的方法」と呼ぶ）が想定可能である。情報処理装置９００が、第２の一般的方法を用いた場合、情報処理装置９００は、マニピュレータ２１０と潜在的な障害物との衝突を回避することできる。しかし、情報処理装置９００が、第２の一般的方法を用いた場合、マニピュレータ２１０の動作範囲に不必要な制限が生じる。

この動作範囲のおける不必要な制限を低減するためには、新たな作業が必要となる。例えば、操作者が、操作装置３０を操作して、情報処理装置９００に対して、新たなマニピュレータ２１０の目標状態（例えば、位置及び姿勢）を設定する必要があった。しかし、操作者は、未観測領域の情報を持っていない。そのため、操作者は、新たに設定する目標状態が未観測領域の観測に適切である否か、及び、新たに設定する目標状態に到達可能であるか否かを判断できない。つまり、第２の一般的な方法は、第１の一般的な方法と同様に、未観測領域の観測に適した経路を出力でいないという問題点があった。

本発明における第１の実施形態に係る情報処理装置１００は、上記の問題点を解決する。

図５は、本発明のおける第１の実施形態に係る情報処理装置１００の構成の一例を示すブロック図である。

情報処理装置１００は、経路生成部１１０と、未観測領域観測計画部１２０と、制御入力演算部１３０とを含む。

経路生成部１１０は、目標状態と、地図情報と、マニピュレータ２１０の現在状態とを基に、マニピュレータ２１０の経路を生成する。そして、経路生成部１１０は、生成した経路を未観測領域観測計画部１２０に送信する。

未観測領域観測計画部１２０は、経路生成部１１０から経路を受信する。さらに、未観測領域観測計画部１２０は、地図情報及びマニピュレータ２１０の現在状態を受信する。そして、未観測領域観測計画部１２０は、経路、地図情報、及び、マニピュレータ２１０の現在状態を基に、地図情報における未観測領域を抽出する。

「観測」とは、地図用センサ２２０を用いて地図情報の処理単位における状態を調べることである。地図情報の処理単位における状態とは、「障害物がない状態（フリースペース）」、又は、「障害物がある状態（障害物領域）」である。つまり、観測とは、地図情報における処理単位が、フリースペースであるか、障害物領域であるかを調べることである。そのため、未観測領域とは、フリースペースであるか、障害物領域であるかを観測していない領域である。

以下、処理単位を「ボクセル」と呼ぶ。ただし、これは、地図情報を３次元の情報に限定するものではない。地図情報は、２次元の情報でもよい。

そして、未観測領域観測計画部１２０は、地図情報における未観測領域と、経路生成部１１０が生成した目標状態に到達する経路とが衝突するか否かを予測する。

衝突が予測された場合、未観測領域観測計画部１２０は、衝突が予測された未観測領域における観測を実施するためのエンドエフェクタ２１５の状態（以下、「修正目標状態」と呼ぶ）を計画する。なお、受信した目標状態と修正目標状態との区別を明確にする場合、受信した目標状態を第１の目標状態と呼び、修正目標状態を第２の目標状態と呼ぶ。

ただし、未観測領域観測計画部１２０は、修正目標状態として、次の点を考慮して計画する。
（１）修正目標状態から目標状態へ移動が容易である。
（２）修正目標状態において、地図用センサ２２０における未観測領域を観測の有用性が高い。

そして、未観測領域観測計画部１２０は、計画した修正目標状態に移動でき、かつ、障害物領域及び未観測領域との衝突を回避する経路を計画する。そして、未観測領域観測計画部１２０は、計画した修正経路を制御入力演算部１３０に出力する。なお、経路生成部１１０が生成した経路と修正経路とを区別する場合、経路生成部１１０が生成した経路（Ｗ）を第１の経路と呼び、修正経路を第２の経路と呼ぶ。

衝突が予測されない場合、未観測領域観測計画部１２０は、制御入力演算部１３０に、経路生成部１１０から受信した経路（第１の経路）を送信する。

制御入力演算部１３０は、受信した経路に沿った制御入力を演算し、演算結果である制御入力をマニピュレータ２１０に送信してマニピュレータ２１０を制御する。この制御において、制御入力演算部１３０は、マニピュレータ２１０から受信したマニピュレータ関節情報を用いる。

つまり、制御入力演算部１３０は、第１の経路又は第２の経路を基にマニピュレータ２１０を制御する。

なお、制御入力演算部１３０は、制御入力の演算方法を制限されない。制御入力演算部１３０は、一般的な手法を用いればよい。そのため、制御入力演算部１３０の詳細な説明を省略する。

［動作の説明］
次に、図面を参照して、第１の実施形態に係る情報処理装置１００の動作について説明する。

まず、図面を参照して、経路生成部１１０の動作について説明する。

図６は、第１の実施形態に係る経路生成部１１０の動作の一例を示すフローチャートである。

経路生成部１１０は、目標状態（Ｐ）を取得する（ステップＳ１０１）。目標状態（Ｐ）とは、目標となるマニピュレータ２１０のエンドエフェクタ２１５の状態（例えば、位置及び姿勢）である。ただし、目標状態（Ｐ）は、マニピュレータ２１０の他の構成の状態を含んでもよい。

経路生成部１１０は、取得した目標状態（Ｐ）にマニピュレータ２１０を移動させるための経路（Ｗ）を生成する（ステップＳ１０２）。つまり、経路生成部１１０は、マニピュレータ２１０の第１の目標状態と、地図情報と、マニピュレータ２１０の現在状態とを基に、第１の目標状態への第１の経路を生成する。

経路生成部１１０は、経路（Ｗ）として、エンドエフェクタ２１５に関する経路（Ｗ）に加え、マニピュレータ２１０の関節部２１２の状態（例えば、関節角）を含む経路（Ｗ）を生成する。ただし、経路生成部１１０は、経路（Ｗ）の生成において、未観測領域をフリースペースとして扱う。なお、経路生成部１１０が経路（Ｗ）の生成に用いる手法は、特に制限されない。例えば、経路生成部１１０は、経路（Ｗ）を生成する手法として、ＲＲＴ（Rapidly-exploring random tree）などのサンプリングベースの経路生成手法を用いてもよい。

経路生成部１１０は、経路（Ｗ）が生成できたか否かを判定する（ステップＳ１０３）。

経路（Ｗ）を生成できなかった場合（ステップＳ１０３でＮｏ）、経路生成部１１０は、動作を終了する。なお、経路生成部１１０は、終了する前に、操作装置３０に対して、目標状態（Ｐ）に到達できないことを示す通知、又は、目標状態（Ｐ）の再入力を促すような通知を送信してもよい。

経路（Ｗ）が生成できた場合（ステップＳ１０３でＹｅｓ）、経路生成部１１０は、生成した経路（Ｗ）を未観測領域観測計画部１２０に送信する。

そして、後ほど説明するように、未観測領域観測計画部１２０が、修正目標状態（Ｐ＾）に対応した経路（Ｗ）を生成する場合、経路生成部１１０は、動作を終了する。

ここで、「Ｐ＾」は、Ｐの上部に「＾（以下、「ハット」と呼ぶ）」を付けた変数を示す。

一方、経路生成部１１０が、修正目標状態（Ｐ＾）に対応した経路（Ｗ）を作成する場合、経路生成部１１０は、以下の動作を実行する。

経路（Ｗ）を送信後、経路生成部１１０は、マニピュレータ２１０（詳細には、エンドエフェクタ２１５）が目標状態（Ｐ）に到達したか否か確認する（ステップＳ１０５）。経路生成部１１０における到達の確認手法は、特に制限されない。例えば、経路生成部１１０は、制御入力演算部１３０から到達についての情報を取得してもよい。あるいは、経路生成部１１０は、マニピュレータ２１０の現在状態（ｐ_ｅ）を基に、到達を確認してもよい。

エンドエフェクタ２１５が、目標状態（Ｐ）に到達した場合（ステップＳ１０５でＹｅｓ）、経路生成部１１０は、動作を終了する。

目標状態（Ｐ）に達していない場合、経路生成部１１０は、未観測領域観測計画部１２０から経路（Ｗ）の生成（再生成）の依頼があるか否かを確認する（ステップＳ１０６）。

依頼がない場合(ステップＳ１０６でＮｏ)、経路生成部１１０は、Ｓ１０５に戻る。

依頼がある場合(ステップＳ１０６でＹｅｓ)、経路生成部１１０は、Ｓ１０２に戻る。つまり、経路生成部１１０は、その時点のマニピュレータ２１０の現状状態から目標状態（Ｐ）までの経路（Ｗ）を生成し、未観測領域観測計画部１２０に送信する。

なお、後ほど説明するように、未観測領域観測計画部１２０は、修正目標状態（Ｐ＾）において、経路（Ｗ）の生成を依頼する。そのため、ここで生成される経路（Ｗ）を最初に生成された経路（Ｗ）（第１の経路）と区別する場合、第３の経路と呼ぶ。

次に、図面を参照して、未観測領域観測計画部１２０の動作について説明する。

未観測領域観測計画部１２０は、観測が必要な未観測領域とマニピュレータ２１０の構造とを基に、未観測領域を観測するための修正目標状態（Ｐ＾）として、マニピュレータ２１０の現在状態（ｐ_ｅ）から到達可能な修正目標状態（Ｐ＾）を生成する。

ただし、未観測領域観測計画部１２０は、後ほど詳細に説明するように、修正目標状態（Ｐ＾）の生成において、コストを用いる。このコストは、現在状態（ｐ_ｅ）から修正目標状態（Ｐ＾）への移動に関するコスト、及び、修正目標状態（Ｐ＾）から目標状態（Ｐ）に移動するコストである。

図７は、第１の実施形態に係る未観測領域観測計画部１２０の全体動作の一例を示すフローチャートである。

図８は、第１の実施形態に係る未観測領域観測計画部１２０における修正目標状態（Ｐ＾）を生成する動作の一例を示すフローチャートである。

図９ないし１６は、マニピュレータ２１０を模式的に示す図である。図９ないし１６において、各ブロックが、ボクセルに対応する。白いボクセルは、フリースペースである。黒いボクセルは、障害物領域である。斜線のボクセルは、未観測領域である。網掛けのボクセルは、修正目標状態（Ｐ＾）の候補を生成するボクセルである。

なお、地図情報は、上記のようにボクセルに関する情報を含む場合、ボクセルマップと呼ばれる。

未観測領域観測計画部１２０は、経路（Ｗ）を取得する（ステップＳ２０１）。なお、既に説明したとおり、経路（Ｗ）は、エンドエフェクタ２１５の経路と、関節部２１２の状態（例えば、関節角）とを含む。なお、経路（Ｗ）は、目標状態（Ｐ）までの経路（Ｗ）のため、目標状態（Ｐ）を含む。

未観測領域観測計画部１２０は、地図情報及びマニピュレータ２１０の現在状態（ｐ_ｅ）を取得する（ステップＳ２０２）。マニピュレータ２１０の現在状態（ｐ_ｅ）は、少なくともエンドエフェクタ２１５の現在状態（ｐ_ｅ）を含む。

未観測領域観測計画部１２０は、地図情報から、障害物領域のボクセルと、未観測領域のボクセルとを抽出する。そして、未観測領域観測計画部１２０は、障害物領域のボクセルの集合（障害物領域集合（ＯＶ））と、未観測領域のボクセルの集合（未観測領域集合（ＵＶ））とを生成する（ステップＳ２０３）。

なお、第１に実施形態に係る集合の形式は、特に制限されない。例えば、各ボクセルが識別子（ＩＤ：Identifier）を持つ場合、集合は、ボクセルのＩＤの集合でもよい。例えば、地図が３次元の場合、ボクセルのＩＤは、地図上のボクセルの座標（ｘ座標、ｙ座標、ｚ座標）でもよい。さらに、ボクセルのＩＤとして３次元の座標を用いる場合、ＩＤは、例えば、「（ｉ，ｊ，ｋ）：ただし、ｉはｘ座標、ｊはｙ座標、ｋはｚ座標である」の形式でもよい。ここで、ＩＤが（ｉ，ｊ，ｋ）であるボクセル（Ｖ）は、「Ｖ_{（ｉ，ｊ，ｋ）}」とすると、各集合は、例えば、次のように表すことができる。
障害物領域集合（ＯＶ）：＝｛Ｖ_{（ｉ１，ｊ１，ｋ１）}｝：ただし、（ｉ_１，ｊ_１，ｋ_１）は、障害物領域のボクセルのＩＤである。
未観測領域集合（ＵＶ）：＝｛Ｖ_{（ｉ２，ｊ２，ｋ２）}｝：ただし、（ｉ_２，ｊ_２，ｋ_２）は、未観測領域のボクセルのＩＤである。
なお、上記において、「：＝」は定義を示し、「｛｝」は集合を示す。

そして、未観測領域観測計画部１２０は、経路（Ｗ）と衝突する未観測領域集合（ＵＶ）の集合を生成する（ステップＳ２０４）。以下、衝突が発生する未観測領域のボクセルの集合を、「衝突未観測領域集合（ＣＵＶ）」と呼ぶ。

未観測領域観測計画部１２０における衝突計算方法は、特に制限はない。例えば、未観測領域観測計画部１２０は、衝突計算として、Ｂｏｕｎｄｉｎｇ Ｂｏｘ法を用いてもよい。Ｂｏｕｎｄｉｎｇ Ｂｏｘ法は、衝突を計算する構成をＢｏｕｎｄｉｎｇ Ｂｏｘと呼ばれる直方体で覆い、覆ったＢｏｕｎｄｉｎｇ Ｂｏｘ群と衝突を判定する領域との衝突を計算する方法である。つまり、未観測領域観測計画部１２０は、マニピュレータ２１０のリンク２１１、関節部２１２、及びエンドエフェクタ２１５を、それぞれＢｏｕｎｄｉｎｇ Ｂｏｘで覆い、Ｂｏｕｎｄｉｎｇ Ｂｏｘ群と未観測領域のボクセルとの衝突を計算してもよい。

そして、未観測領域観測計画部１２０は、衝突が発生した未観測領域のボクセルの集合を抽出して、衝突未観測領域集合（ＣＵＶ）を生成する。

ただし、未観測領域観測計画部１２０は、衝突未観測領域集合（ＣＵＶ）の作成において、衝突が発生する時刻を含む集合とする。衝突未観測領域集合（ＣＵＶ）は、例えば、次のように定義される。なお、衝突未観測領域集合（ＣＵＶ）は、単に、衝突未観測領域とも呼ぶ。
衝突未観測領域集合（ＣＵＶ）：＝｛（Ｖ_{（ｉ３，ｊ３，ｋ３）}，ｔ）｝：ただし、（ｉ_３，ｊ_３，ｋ_３）は、衝突が発生する未観測領域のボクセルのＩＤであり、「t」は衝突の発生する時刻である。

つまり、衝突未観測領域集合（ＣＵＶ）は、未観測領域集合（ＵＶ）からマニピュレータ２１０との衝突が発生する部分集合を抽出し、衝突が発生する時刻を付加した集合である。

図９は、現在状態（ｐ_ｅ）と、未観測領域の衝突とを模式的に示す図である。

そして、未観測領域観測計画部１２０は、衝突未観測領域集合（ＣＵＶ）を作成できたか否か、つまり、衝突が発生したか否かを判定する（ステップＳ２０５）。

衝突が発生しない（ステップＳ２０５でＮｏ）、未観測領域観測計画部１２０は、経路（Ｗ）を制御入力演算部１３０に送信する（ステップＳ２１９）。そして、未観測領域観測計画部１２０は、処理を終了する。なお、この場合、衝突未観測領域集合（ＣＵＶ）は、空集合（ＣＵＶ＝φ）となる。

衝突が発生する場合（ステップＳ２０５でＹｅｓ）、未観測領域観測計画部１２０は、修正目標状態（Ｐ＾）を生成する（ステップＳ２２０）。

図８を参照して、修正目標状態（Ｐ＾）を生成する動作の詳細を説明する。

未観測領域観測計画部１２０は、入力空間を離散化した近似を用いて、数式１に示されている制約付き最適化問題の解として、修正目標状態（Ｐ＾）を算出する。

なお、数式１の最適化問題は、情報処理装置１００に予め与えられている問題である。情報処理装置１００は、図示しない記憶部などに最適化問題を保持している。未観測領域観測計画部１２０は、保持されている最適化問題を用いて以下で説明する動作を実行する。

なお、数式１の第１式は、最適化問題における評価関数である。この最適化問題は、評価関数を最大化する修正目標状態（Ｐ＾）を決定する問題である。

評価関数において、関数Ｅ（Ｐ＾）は、エンドエフェクタ２１５が修正目標状態（Ｐ＾）に移動した場合に、地図用センサ２２０が観測する未観測領域の観測数である。なお、観測が不確定性を含む場合、観測数は、期待値でもよい。つまり、観測期待値とは、地図用センサ２２０が観測できる未観測領域の領域（例えば、ボクセル）の数の期待値である。以下の説明は、期待値を用いる。

関数Ｄ_１（ｐ_ｅ，Ｐ＾）は、現在状態（ｐ_ｅ）から修正目標状態（Ｐ＾）に移動する場合に必要となるコストを表す関数である。関数Ｄ_２（Ｐ＾，Ｐ）は、修正目標状態（Ｐ＾）から目標状態（Ｐ）に移動する場合に必要となるコストを表す関数である。ｗ_１、ｗ_２、及びｗ_３は、それぞれの関数に関する重みである。

評価関数は、「修正目標状態（Ｐ＾）における未観測領域の観測期待値」から、「現在状態（ｐ_ｅ）から修正目標状態（Ｐ＾）への移動に必要なコスト」及び「修正目標状態（Ｐ＾）から目標状態（Ｐ）への移動に必要なコスト」を引く関数である。評価関数は、観測できる未観測領域の期待値が高い修正目標状態（Ｐ＾）、つまり、未観測領域を多く観測できる修正目標状態（Ｐ＾）を高く評価する。さらに、評価関数は、「現在状態（ｐ_ｅ）から修正目標状態（Ｐ＾）への移動に必要なコスト」が低い修正目標状態（Ｐ＾）、つまり、現在状態（ｐ_ｅ）から容易に移動できる修正目標状態（Ｐ＾）を高く評価する。同様に、評価関数は、「修正目標状態（Ｐ＾）から目標状態（Ｐ）への移動に必要なコスト」が低い修正目標状態（Ｐ＾）、つまり、目標状態（Ｐ）に容易に移動できる修正目標状態（Ｐ＾）を高く評価する。まとめると、評価関数は、現在状態（ｐ_ｅ）から修正目標状態（Ｐ＾）、並びに、修正目標状態（Ｐ＾）から目標状態（Ｐ）の移動が容易であり、かつ、未観測領域を多く観測できる修正目標状態（Ｐ＾）を高く評価する。

ただし、評価関数は、上記の関数の一部を含む関数でもよい。

また、第２式以降の式は、最適化問題における制約条件である。

第１及び第２の制約条件における関数ＩＫ（Ｐ＾）は、エンドエフェクタ２１５が修正目標状態（Ｐ＾）を実現するために必要な関節部２１２の角度（関節角）を求める関数である。ただし、関数ＩＫ（Ｐ＾）は、関節部２１２において実現可能な関節角を出力する。つまり、第２の制約条件は、マニピュレータ２１０が、修正目標条件（Ｐ＾）に移動可能であるという制約である。

第１の制約条件における関数Ｃ（ＩＫ（Ｐ＾），ＵＶ∪ＯＶ）は、エンドエフェクタ２１５が修正目標状態（Ｐ＾）まで移動する経路において、マニピュレータ２１０と、障害物領域のボクセル及び未観測領域のボクセルとの干渉を計算する関数である。具体的には、関数Ｃ（ＩＫ（Ｐ＾），ＵＶ∪ＯＶ）は、衝突が発生しない場合には「０」を、衝突が発生する場合には「０でない値（非０）」を出力する。つまり、第１の制約条件は、マニピュレータ２１０が、修正目標状態（Ｐ＾）に移動する経路にいずれにおいても、障害物領域及び未観測領域との干渉しない（衝突が発生しない）という制約条件である。

第３の制約条件における関数Ｔ（ｐ_ｅ，Ｐ＾）は、エンドエフェクタ２１５の現在状態（ｐ_ｅ）から修正目標状態（Ｐ＾）に移動させる到達可能な経路を出力する。つまり、第３の制約条件は、マニピュレータ２１０が、現在状態（ｐ_ｅ）から修正目標状態（Ｐ＾）に移動する経路が存在するとの制約である。

このように、制約条件は、エンドエフェクタ２１５が、修正目標状態（Ｐ＾）に対して、障害物領域及び未観測領域と干渉することなく実現できること、及び、現在状態（ｐ_ｅ）から移動できることを課している。そのため、上記の最適化問題を解として得られた修正目標状態（Ｐ＾）は、マニピュレータ２１０において実行可能な状態となる。

次に、上記の動作を詳細に説明する。

未観測領域観測計画部１２０は、衝突未観測領域集合（ＣＵＶ）から部分集合（以下、早期衝突未観測領域集合（ＣＵＶ_Ｓ）を抽出する。

ここで、早期衝突未観測領域集合（ＣＵＶ_Ｓ）とは、衝突未観測領域集合（ＣＵＶ）に含まれるボクセルの中から、衝突時刻（ｔ）が早い方から順に所定数のボクセルを抽出した集合である。

つまり、未観測領域観測計画部１２０は、経路生成部１１０が生成した経路（第１の経路）と、地図情報と、マニピュレータ２１０の現在の状態とを基に、第１の経路における未観測領域を抽出する。

次に、未観測領域観測計画部１２０は、早期衝突未観測領域集合（ＣＵＶ_Ｓ）の近傍のフリースペースのボクセルの集合である近傍フリー集合（Ｓ）を生成する（ステップＳ２０６）。例えば、未観測領域観測計画部１２０は、早期衝突未観測領域集合（ＣＵＶ_Ｓ）に含まれるボクセルの所定の近傍範囲のボクセルの中から、フリースペースであるボクセルを抽出して近傍フリー集合（Ｓ）を作成してもよい。ここで近傍とは、例えば、３次元のボクセルの場合、直接接続する６個のボクセル（上下、左右、前後のボクセル）の範囲（６近傍）、又は、ボクセルを中心とした上下、左右、前後方向の２７個のボクセルの範囲（２６近傍）である。

図１０は、近傍フリー集合（Ｓ）の一例を示す図である。図１０において、網掛けされたボクセルが、近傍フリー集合（Ｓ）に含まれるボクセルである。なお、近傍フリー集合（Ｓ）は、「近傍フリー領域」又は「近傍領域」とも呼ばれる。

つまり、未観測領域観測計画部１２０は、フリースパース（障害物がない領域）から未観測領域の近傍フリー集合（近傍領域）を抽出する。

未観測領域観測計画部１２０は、近傍フリー集合（Ｓ）に含まれるフリースペースのいずれかのボクセルに含まれるように、エンドエフェクタ２１５を移動させる目標である修正目標状態候補（ｐ_ｍ）を生成する（ステップＳ２０７）。未観測領域観測計画部１２０は、ランダム、又は、所定の算出手法を用いて、修正目標状態候補（ｐ_ｍ）を生成する。

図１１は、修正目標状態候補（ｐ_ｍ）の一例を示す図である。図１１は、参考として、３つの修正目標状態候補（ｐ_ｍ）を示している。なお、図１１において、各ボクセルは、一つの修正目標状態候補（ｐ_ｍ）を含んでいるが、これは、例示である。未観測領域観測計画部１２０は、一つのボクセルに複数の修正目標状態候補（ｐ_ｍ）を生成してもよい。さらに、修正目標状態候補（ｐ_ｍ）が生成されないボクセルがあってもよい。

未観測領域観測計画部１２０は、エンドエフェクタ２１５が修正目標状態候補（ｐ_ｍ）を実現するために必要なとなる関節部２１２の関節角を算出する（ステップＳ２０８）。つまり、未観測領域観測計画部１２０は、関数ＩＫ（ｐ_ｍ）を算出する。これの動作は、第２の制約条件の確認動作でもある。

なお、関節部２１２の関節角などからエンドエフェクタ２１５の位置及び姿勢を求めることは、一般的に「運動学」と呼ばれている。その対応として、エンドエフェクタ２１５の状態から関節角などを求めることは、「逆運動学」と呼ばれている。未観測領域観測計画部１２０は、関数ＩＫ（ｐ_ｍ）の算出において、一般的な逆運動学の手法を用いて関節角を算出できる。

未観測領域観測計画部１２０は、関節角を用いて修正目標状態候補（ｐ_ｍ）を実現するために必要となるマニピュレータ２１０と、障害物領域及び未観測領域との衝突を計算する（ステップＳ２０９）。つまり、未観測領域観測計画部１２０は、第１の制約条件を確認する。ただし、未観測領域観測計画部１２０は、関数ＩＫ（Ｐ＾）として、関数ＩＫ（ｐ_ｍ）を用いる。また、未観測領域観測計画部１２０は、衝突の計算の手法として、ステップＳ２０４において用いた手法を用いればよい。

未観測領域観測計画部１２０は、衝突が発生するか否かを判定する（ステップＳ２１０）。

衝突が発生する場合（ステップＳ２１０でＹｅｓ）、マニピュレータ２１０は、修正目標状態候補（ｐ_ｍ）を実現できない。そのため、未観測領域観測計画部１２０は、修正目標状態候補（ｐ_ｍ）を破棄する（ステップＳ２１３）。そして、未観測領域観測計画部１２０は、ステップＳ２０７に戻る。

衝突が発生しない場合（ステップＳ２１０でＮｏ）、未観測領域観測計画部１２０は、現在状態（ｐ_ｅ）から修正目標状態候補（ｐ_ｍ）に到達する修正経路候補（Ｗ_ｍ）を生成する（ステップＳ２１１）。つまり、未観測領域観測計画部１２０は、第３の制約条件を確認する。なお、未観測領域観測計画部１２０は、修正経路候補（Ｗ_ｍ）の生成に用いる手法として、経路生成部１１０が用いている手法を用いればよい。ただし、未観測領域観測計画部１２０は、未観測領域を障害物とみなして修正経路候補（Ｗ_ｍ）を生成する。つまり、未観測領域観測計画部１２０は、未観測領域に入らないように修正経路候補（Ｗ_ｍ）を生成する。

図１２は、修正経路候補（Ｗ_ｍ）の一例を示す図である。図１２は、参考として、波線を用いて、一つの修正目標状態候補（ｐ_ｍ）におけるマニピュレータ２１０の状態を示している。

なお、経路生成部１１０が、修正経路候補（Ｗ_ｍ）を生成してもよい。この場合、未観測領域観測計画部１２０は、修正目標状態候補（ｐ_ｍ）を経路生成部１１０に送信する。そして、経路生成部１１０は、新たな経路として、修正経路候補（Ｗ_ｍ）を生成し、生成した修正経路候補（Ｗ_ｍ）を未観測領域観測計画部１２０に送信する。図５は、この場合の含むように、未観測領域観測計画部１２０と経路生成部１１０との間に修正目標状態候補（ｐ_ｍ）を示している。ただし、図５は、未観測領域観測計画部１２０が経路を生成する場合を除外するものではない。

未観測領域観測計画部１２０は、修正経路候補（Ｗ_ｍ）が生成できたか否かを判定する（ステップＳ２１２）。

修正経路候補（Ｗ_ｍ）を生成できない場合（ステップＳＳ２１２でＮｏ）、マニピュレータ２１０は、現在状態（ｐ_ｅ）から修正目標状態候補（ｐ_ｍ）に移動できない。つまり、この場合は、第３の制約条件を満足できない場合である。そのため、未観測領域観測計画部１２０は、修正目標状態候補（ｐ_ｍ）を破棄する（ステップＳ２１３）。そして、未観測領域観測計画部１２０は、ステップＳ２０７に戻る。

修正経路候補（Ｗ_ｍ）が生成できた場合（ステップＳ２１２でＹｅｓ）、未観測領域観測計画部１２０は、修正目標状態候補（ｐ_ｍ）のスコア（以下、評価値（ｃ_ｍ）とする）を算出する（ステップＳ２１４）。

なお、評価値（ｃ_ｍ）は、評価関数と同型である。すなわち、未観測領域観測計画部１２０は、次に示す数式２を用いて、修正目標状態候補（ｐ_ｍ）の評価値（ｃ_ｍ）を算出する。

関数Ｅ（ｐ_ｍ）は、修正目標状態候補（ｐ_ｍ）において、未観測領域における観測可能な領域の数の期待値である。具体的には、この期待値は、修正目標状態候補（ｐ_ｍ）において、未観測領域の含まれるボクセルの中で観測できるボクセルの数の期待値である。未観測領域観測計画部１２０は、期待値の算出手法を制限されない。例えば、未観測領域観測計画部１２０は、この期待値の計算として、地図用センサ２２０の性能（例えば、画角）を基に、Ｒａｙｃａｓｔ法を用いて、修正目標状態候補（ｐ_ｍ）から観測が期待される未観測領域のボクセル数を算出してもよい。なお、Ｒａｙｃａｓｔ法とは、仮想的な線を利用して衝突検知を行う方法である。

関数Ｄ_１（ｐ_ｅ、ｐ_ｍ）は、修正経路候補（Ｗ_ｍ）に沿った現在状態（ｐ_ｅ）から修正目標状態候補（ｐ_ｍ）への移動に要するコストである。関数Ｄ_２（ｐ_ｍ、Ｐ）は、修正目標状態候補（ｐ_ｍ）から目標状態（Ｐ）への移動に要するコストである。未観測領域観測計画部１２０は、これらのコストの算出として、一般的なコスト計算手法を用いてもよい。例えば、未観測領域観測計画部１２０は、関数Ｄ_１（ｐ_ｅ、ｐ_ｍ）として、現在状態（ｐ_ｅ）から修正目標状態候補（ｐ_ｍ）への修正経路候補（Ｗ_ｍ）の経路長を用いてもよい。あるいは、未観測領域観測計画部１２０は、関数Ｄ_２（ｐ_ｍ，Ｐ）として、２つの状態間のユークリッド距離を用いてもよい。

図１３は、修正目標状態候補（ｐ_ｍ）の評価値（ｃ_ｍ）を算出する経路と、地図用センサ２２０のセンス範囲との一例を示す図である。

未観測領域観測計画部１２０は、所定数（ｎ）まで、評価値（ｃ_ｍ）を算出した修正目標状態候補（ｐ_ｍ）の算出を繰り返す。

そのため、未観測領域観測計画部１２０は、評価値（ｃ_ｍ）を算出した修正目標状態候補（ｐ_ｍ）を所定数（ｎ）まで生成したか否かを判定する（ステップＳ２１５）。

所定数の修正目標状態候補（ｐ_ｍ）を生成していない場合（ステップＳ２１５でＮｏ）、未観測領域観測計画部１２０は、ステップＳ２０７に戻る。

所定数の修正目標状態候補（ｐ_ｍ）を生成した場合（ステップＳ２１５でＹｅｓ）、未観測領域観測計画部１２０は、修正目標状態（Ｐ＾）として、最も評価値（ｃ_ｍ）が高い修正目標状態候補（ｐ_ｍ）を選択する（ステップＳ２１６）。そして、未観測領域観測計画部１２０は、修正経路（Ｗ＾）として、選択した修正目標状態候補（ｐ_ｍ）の修正経路候補（Ｗ_ｍ）を選択する。そして、未観測領域観測計画部１２０は、修正目標状態（Ｐ＾）と修正経路（Ｗ＾）とを制御入力演算部１３０に出力する。

数式を用いて、この場合の修正目標状態（Ｐ＾）を表すと、次の数式３となる。

数式３において、「^＊」は、選択されたことを示す記号である。つまり、未観測領域観測計画部１２０は、修正目標状態（Ｐ＾）として、評価値（ｃ_ｍ）を最大とする修正目標状態候補（ｐ_ｍ＊）を選択する。そして、未観測領域観測計画部１２０は、修正目標状態（Ｐ＾）に対応した修正経路（Ｗ＾）として、選択した修正目標状態候補（ｐ_ｍ＊）の修正経路候補（Ｗ_ｍ＊）を選択する。

図１４は、修正目標状態（Ｐ＾）として選択された修正目標状態候補（ｐ_ｍ＊）を示す図である。

つまり、未観測領域観測計画部１２０は、近傍領域において未観測領域を観測させるための修正目標状態（Ｐ＾）（第２の目標状態）を生成し、マニピュレータ２１０の現状状態からの第２の目標状態までの修正経路（Ｗ＾）（第２の経路）を生成する。

ここまでの動作を用いて、未観測領域観測計画部１２０は、修正目標状態（Ｐ＾）を生成する。

このように、未観測領域観測計画部１２０は、近傍フリー集合（Ｓ）（近傍領域）において、修正目標状態（Ｐ＾）（第２の目標状態）の候補である複数の修正目標状態候補（ｐ_ｍ）（以下、単に「目標状態候補」とも呼ぶ）を生成する。そして、未観測領域観測計画部１２０は、第２の目標状態として、目標状態候補の評価値（ｃ_ｍ）を最適化する目標状態候補を選択する。

図７を用いた説明に戻る。

未観測領域観測計画部１２０は、マニピュレータ２１０のエンドエフェクタ２１５が、修正目標状態（Ｐ＾）に到達したか否かを判定する（ステップＳ２１７）。

到達していない場合（ステップＳ２１７でＮｏ）、未観測領域観測計画部１２０は、到達するまで、判定を繰り返す。つまり、未観測領域観測計画部１２０は、修正目標状態（Ｐ＾）への到達を待つ。

図１５は、エンドエフェクタ２１５が修正目標状態（Ｐ＾）に到達した状態を示す図である。

エンドエフェクタ２１５が修正目標状態（Ｐ＾）に到達した場合（ステップＳ２１７でＹｅｓ）、未観測領域観測計画部１２０は、経路生成部１１０に経路（Ｗ）の生成（再生成）を依頼する（ステップＳ２１８）。

この状態では、エンドエフェクタ２１５は、修正目標状態（Ｐ＾）に位置する。そのため、経路生成部１１０は、修正目標状態（Ｐ＾）から目標状態（Ｐ）への経路（Ｗ）を生成する。なお、ここで生成された経路（Ｗ）、つまり、再生成された経路（Ｗ）を、既に説明したように第３の経路と呼ぶ。

そして、未観測領域観測計画部１２０は、ステップＳ２１０に戻る。

このように、未観測領域観測計画部１２０は、修正目標状態（Ｐ＾）を新たな現在状態（ｐ_ｅ）として、同様の処理を繰り返す。そのため、未観測領域観測計画部１２０は、修正目標状態（Ｐ＾）と目標状態（Ｐ）との間に未観測領域が残っていても、目標状態（Ｐ）までの適切な経路（Ｗ）を出力することができる。

図１６は、目標状態（Ｐ）に到達したマニピュレータ２１０を示す図である。

［効果の説明］
次に、第１の実施形態の効果について説明する。

第１の実施形態に係る情報処理装置１００は、未観測領域の観測に適した経路を出力するとの効果を奏することができる。

その理由は、次のとおりである。

情報処理装置１００の経路生成部１１０は、目標状態（Ｐ）を基に経路（Ｗ）を生成する。

未観測領域観測計画部１２０は、経路（Ｗ）と、地図情報と、マニピュレータ２１０の現在状態（ｐ_ｅ）とを基に、経路（Ｗ）と衝突する未観測領域である衝突未観測領域集合（ＣＵＶ）を抽出する。そして、未観測領域観測計画部１２０は、衝突未観測領域の近傍のフリースペースから近傍フリー集合（Ｓ）を抽出する。

そして、未観測領域観測計画部１２０は、近傍フリー集合（Ｓ）においてエンドエフェクタ２１５を移動させる目標である修正目標状態（Ｐ＾）を生成する。そして、未観測領域観測計画部１２０は、現在状態（ｐ_ｅ）から修正目標状態（Ｐ＾）までの経路である修正経路（Ｗ＾）を算出する。

そして、制御入力演算部１３０が、修正経路（Ｗ＾）を基にマニピュレータ２１０を制御するためである。

さらに、未観測領域観測計画部１２０は、修正目標状態（Ｐ＾）の候補として、複数の修正目標状態候補（ｐ_ｍ）を生成する。そして、未観測領域観測計画部１２０は、修正目標状態（Ｐ＾）として、修正目標状態候補（ｐ_ｍ）の中から、未観測領域における観測数が多く、移動のコストが低い修正目標状態候補（ｐ_ｍ）を選択する。

そのため、制御入力演算部１３０は、未観測領域を減らし、移動のコストが低いようにマニピュレータ２１０を制御できるためである。

さらに、第１の実施形態に係る情報処理装置１００は、情報処理装置１００を含む操作システム１０の操作者に対して、障害物との衝突回避が必要な状況においても、容易にマニピュレータ２１０の操作できるとの効果を奏することができる。

その理由は、次のとおりである。

情報処理装置１００は、操作者からの目標状態（Ｐ）を基に上記の動作を実行できる。そのため、情報処理装置１００は、操作者に修正目標状態（Ｐ＾）を意識させない。

さらに、情報処理装置１００は、操作者の介入を必要としないで、マニピュレータ２１０と障害物との衝突を回避するためである。

さらに、情報処理装置１００の未観測領域観測計画部１２０は、操作者の補助を必要としないで、観測が必要な未観測領域の抽出と観測位置である修正目標状態（Ｐ＾）を算出するためである。

また、未観測領域観測計画部１２０は、近傍フリー集合（Ｓ）における候補について処理を実行する。そして、未観測領域観測計画部１２０は、制約条件を満足する候補について処理を実行する。そのため、情報処理装置１００は、不必要な処理を低減できる。

＜その他の実施形態＞
次に、その他の実施形態について説明する。

未観測領域観測計画部１２０は、経路（Ｗ）と未観測領域との衝突を判定し、衝突が起きる場合に、修正目標状態（Ｐ＾）を生成する。そのため、修正経路（Ｗ＾）は、経路（Ｗ）に沿っていない場合がある。

しかし、未観測領域観測計画部１２０は、衝突の予測されるボクセルの手前まで修正経路（Ｗ＾）として、経路生成部１１０が生成した経路（Ｗ）に用いてもよい。この場合、マニピュレータ２１０が、経路（Ｗ）に沿った移動において、未観測領域に到達すると、その時点で、マニピュレータ２１０のエンドエフェクタ２１５は、修正目標状態（Ｐ＾）に移動したことになる。

あるいは、情報処理装置１００は、操作装置３０に含まれていてもよい。なお、地図生成部２３０が、操作装置３０に含まれていてもよい。

あるいは、情報処理装置１００に含まれる経路生成部１１０、未観測領域観測計画部１２０、及び、制御入力演算部１３０は、それぞれ、マニピュレータ部２０及び／又は操作装置３０に分けて配置されてもよい。

あるいは、情報処理装置１００が、入力装置３１０及び／又は出力装置３２０を含んでもよい。

マニピュレータ２１０は、固定式に限らず、図２に示されているように移動可能な台車等に搭載されてもよい。この場合、情報処理装置１００は、マニピュレータ２１０に加え、台車の動作を制御してもよい。

＜変形例＞
次に、第１の実施形態の変形例について説明する。

以上の説明した情報処理装置１００は、次のように構成される。

例えば、情報処理装置１００の各構成部は、ハードウェア回路で構成されてもよい。

また、情報処理装置１００において、各構成部は、ネットワークを介して接続した複数の装置を用いて、構成されてもよい。

また、情報処理装置１００において、複数の構成部は、１つのハードウェアで構成されてもよい。

また、情報処理装置１００は、ＣＰＵ（Central Processing Unit）と、ＲＯＭ（Read Only Memory）と、ＲＡＭ（Random Access Memory）とを含むコンピュータ装置として実現されてもよい。情報処理装置１００は、上記構成に加え、さらに、内部インターフェース回路（ＩＩＣ：Inner Interface Circuit）と、ネットワークインターフェース回路（ＮＩＣ：Network Interface Circuit）とを含むコンピュータ装置として実現されてもよい。

図１７は、本変形例に係る情報処理装置６００の構成の一例を示すブロック図である。

情報処理装置６００は、ＣＰＵ６１０と、ＲＯＭ６２０と、ＲＡＭ６３０と、内部記憶装置６４０と、ＩＩＣ６５０と、ＮＩＣ６６０とを含み、コンピュータ装置を構成している。

ＣＰＵ６１０は、ＲＯＭ６２０からプログラムを読み込む。そして、ＣＰＵ６１０は、読み込んだプログラムに基づいて、ＲＡＭ６３０と、内部記憶装置６４０と、ＩＩＣ６５０と、ＮＩＣ６６０とを制御する。そして、ＣＰＵ６１０を含むコンピュータは、これらの構成を制御し、図５に示されている、経路生成部１１０と、未観測領域観測計画部１２０と、制御入力演算部１３０としての各機能を実現する。

ＣＰＵ６１０は、各機能を実現する際に、ＲＡＭ６３０又は内部記憶装置６４０を、プログラムの一時記憶媒体として使用してもよい。

また、ＣＰＵ６１０は、コンピュータで読み取り可能にプログラムを記憶した記憶媒体６９０が含むプログラムを、図示しない記憶媒体読み取り装置を用いて読み込んでもよい。あるいは、ＣＰＵ６１０は、ＩＩＣ６５０又はＮＩＣ６６０を介して、図示しない外部の装置からプログラムを受け取り、ＲＡＭ６３０に保存して、保存したプログラムを基に動作してもよい。なお、記憶媒体６９０は、例えば、ＣＤ（Compact Disc）−ＲＯＭ、又は、ＤＶＤ（Digital Versatile Disc）−ＲＯＭである。

ＲＯＭ６２０は、ＣＰＵ６１０が実行するプログラム及び固定的なデータを記憶する。ＲＯＭ６２０は、例えば、Ｐ−ＲＯＭ（Programmable-ROM）又はフラッシュＲＯＭである。

ＲＡＭ６３０は、ＣＰＵ６１０が実行するプログラム及びデータを一時的に記憶する。ＲＡＭ６３０は、例えば、Ｄ−ＲＡＭ（Dynamic-RAM）である。

内部記憶装置６４０は、情報処理装置６００が長期的に保存するデータ及びプログラムを記憶する。また、内部記憶装置６４０は、ＣＰＵ６１０の一時記憶装置として動作してもよい。内部記憶装置６４０は、例えば、ハードディスク装置、光磁気ディスク装置、又は、ＳＳＤ（Solid State Drive）である。

ここで、ＲＯＭ６２０と内部記憶装置６４０は、不揮発性（non-transitory）の記憶媒体である。一方、ＲＡＭ６３０は、揮発性（transitory）の記憶媒体である。そして、ＣＰＵ６１０は、ＲＯＭ６２０、内部記憶装置６４０、又は、ＲＡＭ６３０に記憶されているプログラムを基に動作可能である。つまり、ＣＰＵ６１０は、不揮発性記憶媒体又は揮発性記憶媒体を用いて動作可能である。

ＩＩＣ６５０は、ＣＰＵ６１０と、マニピュレータ部２０に含まれる他の構成部（マニピュレータ２１０及び地図生成部２３０）とのデータを仲介する。ＩＩＣ６５０は、例えば、ＰＣＩ（Peripheral Component Interconnect）カード、ＵＳＢ（Universal Serial Bus）カード、又は、マニピュレータ２１０用の専用回路である。なお、ＩＩＣ６５０は、有線に限らず、無線を用いてもよい。

ＮＩＣ６６０は、所定の通信路を介して操作装置３０とのデータのやり取りを中継する。ＮＩＣ６６０は、例えば、無線ＬＡＮ（Local Area Network）カードである。ただし、ＮＩＣ６６０は、無線の通信路に限らず、有線の通信路を用いてもよい。

このように構成された情報処理装置６００は、情報処理装置１００と同様の効果を得ることができる。

その理由は、情報処理装置６００のＣＰＵ６１０が、プログラムに基づいて情報処理装置１００と同様の機能を実現できるためである。

以上、実施形態を参照して本願発明を説明したが、本願発明は上記実施形態に限定されるものではない。本願発明の構成及び詳細には、本願発明のスコープ内で当業者が理解し得る様々な変更をすることができる。

本発明は、産業用マニピュレータ、又は、極限環境（例えば、原発内又は災害現場）において動作する作業用ロボットに利用可能である。

１０ 操作システム
２０ マニピュレータ部
３０ 操作装置
４０ 対象物
１００ 情報処理装置
１１０ 経路生成部
１２０ 未観測領域観測計画部
１３０ 制御入力演算部
２１０ マニピュレータ
２１１ リンク
２１２ 関節部
２１３ 基台
２１５ エンドエフェクタ
２２０ 地図用センサ
２３０ 地図生成部
３１０ 入力装置
３２０ 出力装置
６００ 情報処理装置
６１０ ＣＰＵ
６２０ ＲＯＭ
６３０ ＲＡＭ
６４０ 内部記憶装置
６５０ ＩＩＣ
６６０ ＮＩＣ
６９０ 記憶媒体
９００ 情報処理装置
９１０ 経路生成部
９３０ 制御入力演算部

Claims (8)

1. マニピュレータの第１の目標状態と、地図情報と、前記マニピュレータの現在状態とを基に、前記第１の目標状態への第１の経路を生成する経路生成手段と、
   前記第１の経路と、前記地図情報と、前記現在状態とを基に、前記第１の経路における未観測領域を抽出し、障害物がない領域から前記未観測領域の近傍領域を抽出し、前記近傍領域において前記未観測領域を観測させる第２の目標状態を生成し、前記現在状態から前記第２の目標状態までの第２の経路を生成する未観測領域観測計画手段と、
   前記第１の経路又は前記第２の経路を基に前記マニピュレータを制御する制御入力演算手段と
   を含む情報処理装置。
2. 前記未観測領域観測計画手段が、
   前記近傍領域において、前記第２の目標状態の候補である複数の目標状態候補を生成し、
   前記第２の目標状態として、前記目標状態候補の評価値を最適化する前記目標状態候補を選択する
   請求項１に記載の情報処理装置。
3. 前記評価値が、
   前記目標状態候補において観測できる前記未観測領域の数と、
   前記現在状態から前記目標状態候補まで移動するコストと、
   前記目標状態候補から前記第１の目標状態まで移動するコストとの少なくとも一つを含む
   請求項２に記載の情報処理装置。
4. 前記未観測領域観測計画手段が、
   前記目標状態候補として、
   前記マニピュレータが前記目標状態候補に移動可能であり、
   前記マニピュレータが前記目標状態候補に移動する前記第２の経路のいずれにおいても、前記障害物を含む領域及び前記未観測領域と干渉せず、
   かつ、
   前記マニピュレータが、前記現在状態から前記目標状態候補に移動可能である
   前記目標状態候補を生成する
   請求項２又は３に記載の情報処理装置。
5. 前記未観測領域観測計画手段が、
   前記近傍領域においてランダムに、所定数以上の前記目標状態候補を生成する
   請求項２ないし４のいずれか１項に記載の情報処理装置。
6. 前記未観測領域観測計画手段が
   前記マニピュレータが、前記第２の目標状態に到達した場合に、前記経路生成手段に前記第２の目標状態から前記第１の目標状態までの第３の経路の生成を依頼し、
   生成され前記第３の経路を基に前記第２の経路を算出する
   請求項１ないし５のいずれか１項に記載の情報処理装置。
7. マニピュレータの第１の目標状態と、地図情報と、前記マニピュレータの現在状態とを基に、前記第１の目標状態への第１の経路を生成し、
   前記第１の経路と、前記地図情報と、前記現在状態とを基に、前記第１の経路における未観測領域を抽出し、
   障害物がない領域から前記未観測領域の近傍領域を抽出し、
   前記近傍領域において前記未観測領域を観測させる第２の目標状態を生成し、
   前記現在状態から前記第２の目標状態までの第２の経路を生成し、
   前記第１の経路又は前記第２の経路を基に前記マニピュレータを制御する
   情報処理方法。
8. マニピュレータの第１の目標状態と、地図情報と、前記マニピュレータの現在状態とを基に、前記第１の目標状態への第１の経路を生成する処理と、
   前記第１の経路と、前記地図情報と、前記現在状態とを基に、前記第１の経路における未観測領域を抽出する処理と、
   障害物がない領域から前記未観測領域の近傍領域を抽出する処理と、
   前記近傍領域において前記未観測領域を観測させる第２の目標状態を生成する処理と、
   前記現在状態から前記第２の目標状態までの第２の経路を生成する処理と、
   前記第１の経路又は前記第２の経路を基に前記マニピュレータを制御する処理と
   をコンピュータに実行させるプログラム。

Images