JP7224559B2 - 遠隔制御マニピュレータシステムおよび遠隔制御支援システム - Google Patents

Description

本開示は、マニピュレータを遠隔制御する遠隔制御マニピュレータシステムおよびマニピュレータを遠隔制御することを支援する遠隔制御支援システムに関する。

マニピュレータやロボットなどを遠隔操作することを支援する遠隔操作支援装置が提案されている（特許文献１参照）。遠隔操作支援装置は、ロボットが扱う対象物の３次元モデルおよびロボットの３次元モデルを生成し、３次元空間の指定された視点から見たロボットおよび対象物が表示された画像を生成して、操作者に提示する。対象物の３次元モデルは、レーザスキャナ１３にて計測された対象物１１上の点群データから生成する。ロボットの３次元モデルは、ロボット１２の各軸の状態１９に基づいてロボット１２の動作状態を反映させて生成する。

ロボットやマニピュレータを遠隔制御するために、オペレータが頭に両目を覆うように装着する表示装置であるヘッドマウントディスプレイ（Head Mount Display、ＨＭＤ）を装着するシステムがある（特許文献２参照）。

特開２００９－００６４１０ ＷＯ２０１９／０５９３６４

ロボットにはカメラや各種センサが搭載される。カメラが撮影した画像や各種センサが検出したセンサ情報は、ディスプレイに表示される。オペレータは、ディスプレイに表示された画像やセンサ情報を確認しながらロボットを操作する。遠近も認識できる立体視画像(３Ｄ画像とも呼ぶ)をオペレータが容易に見ることができるように、オペレータはＨＭＤを装着する。オペレータがＨＭＤを頭部に装着する場合には、オペレータが見ることができる情報がＨＭＤを介して提供される情報に限定されるという課題がある。

例えば、ロボットに搭載されたカメラの視野の画像（以下、ロボット視野画像と呼ぶ）を立体視させる場合に、オペレータがＨＭＤを装着する場合には、以下のような課題がある。オペレータの顔の向きによらず常にロボット視野画像を表示すると、ロボット視野画像以外の情報（ロボットの位置情報や姿勢情報、周囲の環境情報など、制御参考情報と呼ぶ）を、オペレータが確認することが難しい。ＨＭＤに３Ｄ画像と制御参考情報を重畳して提供する場合には、制御参考情報の表示サイズや情報量が限定され、３Ｄ画像とそれ以外の情報が重畳されているために画像が見づらい。

制御参考情報として、カメラの視点とは異なる視点から見た対象物およびロボットの画像である別視点画像をＨＭＤに表示する場合を考える。ＨＭＤにロボット視野画像または別視点画像の何れを表示するかを、オペレータが入力する必要があり、オペレータに負担になる。表示システムとしても、オペレータが入力する表示画像の切替指示に対応できる必要があり、システムが複雑になる。

本開示は、マニピュレータが扱う物体を立体的に見る立体視画像と、任意の視点から周囲環境およびマニピュレータを見るモデル画像とを操作者が頭を移動させることで容易に切り替えて見ることができる遠隔制御マニピュレータシステムおよび遠隔制御支援システムを得ることを目的とする。周囲環境とは、マニピュレータが扱う物体および物体の周囲の事物である。

本開示に係る遠隔制御マニピュレータシステムは、操作者により遠隔制御されて物体を扱うマニピュレータと、マニピュレータが動くまたは静止する動作を指示する動作指示を操作者が入力する動作指示入力部と、動作指示からマニピュレータを制御する制御信号を生成する制御信号生成部と、決められた位置関係を有してマニピュレータに搭載された右カメラおよび左カメラと、右カメラが撮影する右目画像および左カメラが撮影する左目画像を立体的に見えるように表示する立体視表示装置と、マニピュレータの位置および姿勢を表す位置姿勢情報を検出する位置姿勢センサと、マニピュレータの構造を表すマニピュレータ構造データ、および右カメラおよび左カメラの構造を表すカメラ構造データを記憶する構造データ記憶部と、マニピュレータ構造データ、カメラ構造データおよび位置姿勢情報を参照して、右カメラおよび左カメラを含むマニピュレータの３次元モデルであるマニピュレータモデルを生成するマニピュレータモデル生成部と、マニピュレータモデルに含まれる右カメラおよび左カメラの位置と撮影方向に基づき、右目画像および左目画像を画像処理して、物体および物体の周囲の事物である周囲環境の３次元モデルである周囲環境モデルを生成する画像処理モデル生成部と、周囲環境モデルとマニピュレータモデルを統合した３次元モデルである統合３次元モデルを生成するモデル統合部と、操作者が指定した視点から統合３次元モデルを見る画像であるモデル画像を生成するモデル画像生成部と、モデル画像を表示する、操作者が頭を移動させることで立体視表示装置と切り替えて見ることができるモデル画像表示装置とを備えたものである。

本開示に係る遠隔制御支援システムは、操作者により遠隔制御されて物体を扱うマニピュレータに搭載されて決められた位置関係を有する右カメラおよび左カメラが撮影する右目画像および左目画像を立体的に見えるように表示する立体視表示装置と、マニピュレータの構造を表すマニピュレータ構造データ、および右カメラおよび左カメラの構造を表すカメラ構造データを記憶する構造データ記憶部と、マニピュレータ構造データ、カメラ構造データおよびマニピュレータの位置および姿勢を表す位置姿勢情報を参照して、右カメラおよび左カメラを含むマニピュレータの３次元モデルであるマニピュレータモデルを生成するマニピュレータモデル生成部と、マニピュレータモデルに含まれる右カメラおよび左カメラの位置と撮影方向に基づき、右目画像および左目画像を画像処理して、物体および物体の周囲の事物である周囲環境の３次元モデルである周囲環境モデルを生成する画像処理モデル生成部と、周囲環境モデルとマニピュレータモデルを統合した３次元モデルである統合３次元モデルを生成するモデル統合部と、操作者が指定した視点から統合３次元モデルを見る画像であるモデル画像を生成するモデル画像生成部と、モデル画像を表示する、操作者が頭を移動させることで立体視表示装置と切り替えて見ることができるモデル画像表示装置とを備えたものである。

本開示によれば、マニピュレータが扱う物体を立体的に見る立体視画像と、任意の視点から物体および物体の周囲の事物である周囲環境およびマニピュレータを見るモデル画像とを操作者が頭を移動させることで容易に切り替えて見ることができる。

実施の形態１に係る遠隔制御マニピュレータシステムの概略構成を説明する図である。 実施の形態１に係る遠隔制御マニピュレータシステムの機能構成を示すブロック図である。 実施の形態１に係る遠隔制御マニピュレータシステムで使用されるロボットの斜視図である。 実施の形態１に係る遠隔制御マニピュレータシステムで使用されるロボットのヒューマノイド部の正面図である。 実施の形態１に係る遠隔制御マニピュレータシステムで使用されるロボットのヒューマノイド部の左側面図である。 実施の形態１に係る遠隔制御マニピュレータシステムで使用されるロボットが有する回転軸を説明する図である。 実施の形態１に係る遠隔制御マニピュレータシステムで使用されるロボットの腕部が有する回転軸を説明する図である。 実施の形態１に係る遠隔制御マニピュレータシステムでロボットを遠隔制御する手順を説明するフローチャートである。 実施の形態１に係る遠隔制御マニピュレータシステムで周囲点群データを初期生成する手順を説明するフローチャートである。 実施の形態１に係る遠隔制御マニピュレータシステムでロボットが物体を把持する際に遠隔制御する手順を説明するフローチャートである。 実施の形態２に係る遠隔制御マニピュレータシステムで使用されるロボットのヒューマノイド部の正面図である。 実施の形態２に係る遠隔制御マニピュレータシステムで使用されるロボットのヒューマノイド部の左側面図である。 実施の形態２に係る遠隔制御マニピュレータシステムの機能構成を示すブロック図である。 実施の形態２に係る遠隔制御マニピュレータシステムでロボットを遠隔制御する手順を説明するフローチャートである。 実施の形態２に係る遠隔制御マニピュレータシステムで周囲点群データを初期生成する手順を説明するフローチャートである。 実施の形態２に係る遠隔制御マニピュレータシステムでロボットが物体を把持する際に遠隔制御する手順の前半を説明するフローチャートである。 実施の形態２に係る遠隔制御マニピュレータシステムでロボットが物体を把持する際に遠隔制御する手順の後半を説明するフローチャートである。 実施の形態３に係る遠隔制御マニピュレータシステムの機能構成を示すブロック図である。 実施の形態３に係る遠隔制御マニピュレータシステムでロボットを遠隔制御する手順を説明するフローチャートである。 実施の形態３に係る遠隔制御マニピュレータシステムでロボットが物体を把持する際に遠隔制御する手順の前半を説明するフローチャートである。 実施の形態３に係る遠隔制御マニピュレータシステムでロボットが物体を把持する際に遠隔制御する手順の後半を説明するフローチャートである。 実施の形態４に係る遠隔制御マニピュレータシステムおよび遠隔制御支援システムの機能構成を示すブロック図である。

実施の形態１．
図１および図２を参照して、遠隔制御マニピュレータシステム１００の概略構成を説明する。図１は、実施の形態１に係る遠隔制御マニピュレータシステムの概略構成を説明する図である。図２は、実施の形態１に係る遠隔制御マニピュレータシステムの機能構成を示すブロック図である。遠隔制御マニピュレータシステム１００は、クローラ移動ロボット１（ロボット１と略す）、右カメラ２Ｒ、左カメラ２L、制御装置３、立体視表示装置４、表示装置５、目標姿勢入力装置６を主に有して構成される。ロボット１は、オペレータ３０により遠隔制御されて物体２０を扱うマニピュレータである。ロボット１が物体２０を扱う場所を現場と呼ぶ。オペレータ３０が居る場所を指令所と呼ぶ。オペレータ３０は、マニピュレータを遠隔制御する操作者である。

右カメラ２Ｒおよび左カメラ２Lは、ロボット１の頭部に搭載されて画像を撮影する。制御装置３は、ロボット１を制御する。立体視表示装置４は、右カメラ２Ｒおよび左カメラ２Lで撮影された画像を立体視表示する。立体視表示するとは、立体的に見えるように表示することである。表示装置５は、ロボット１および周囲環境の３次元モデルを任意の視点から見る画像や、ロボット１を制御する上で必要あるいは有用な情報を表示する。周囲環境は、物体２０および物体２０の周囲の事物である。周囲環境の３次元モデルが、周囲環境モデルである。目標姿勢入力装置６は、オペレータ３０がロボット１を操作する際に、操作後の目標となる位置および姿勢を表す目標姿勢を入力する。目標姿勢は、ロボット１が動くまたは静止する動作をオペレータ３０が指示する動作指示である。目標姿勢は、動作指示に従う動作を実行後のロボット１の位置および姿勢を表す。目標姿勢目標姿勢入力装置６は、動作指示をオペレータ３０が入力する動作指示入力部である。

遠隔制御の対象であるロボットは、図３から図５に示すロボット１である。遠隔制御マニピュレータシステムは、ロボット１とは異なるロボットも遠隔制御することができる。図３は、実施の形態１に係る遠隔制御マニピュレータシステムで使用されるロボットの斜視図である。図４は、実施の形態１に係る遠隔制御マニピュレータシステムで使用されるロボットのヒューマノイド部を拡大した正面図である。図５は、ヒューマノイド部の左側面図である。ロボット１は、車両部１Ｗと、ヒューマノイド部１Ｈと、制御部７とを有する。車両部１Ｗは、クローラ（キャタピラ）を車輪で回転させて移動する車両である。ヒューマノイド部１Ｈは、車両部１Ｗの前方上面に搭載される。ヒューマノイド部１Ｈは、２本の腕部、胴体部および頭部を有する。制御部７は、車両部１Ｗおよびヒューマノイド部１Ｈを制御する。図３から図５に示す姿勢をロボット１がとる状態を、ロボット１の基準状態と呼ぶ。

ＸＹＺ座標系は、ロボット１が存在する空間を表す直交座標系である。ＸＹ平面は水平な平面である。ＸＹＺ座標系を現場座標系とも呼ぶ。Ｘ軸は東西方向に平行で、Ｙ軸は南北方向に平行である。東をＸ軸の正の向きとし、北をＹ軸の正の向きとする。Ｚ軸は、高さ方向の軸である。上方をＺ軸の正の向きとする。位置センサ９１は、車両部１ＷのＸＹＺ座標系での位置と姿勢を計測する。車両部１Ｗの位置は、車両部１Ｗの重心のＸＹＺ座標系での位置である。車両部１Ｗの姿勢は、車両部１Ｗの前方が向く方向である。車両部１Ｗの姿勢は、ヨー角、ピッチ角およびロール角で表現する。車両部１Ｗの位置および姿勢を表すデータを、車両姿勢５６と呼ぶ。

車両部１Ｗの姿勢を表現するために、車両部１Ｗとともに移動する第1の直交座標系（Ｘ１Ｙ１Ｚ１座標系）を以下のように定義する。第１の直交座標系は、ヒューマノイド部１Ｈの車両部１Ｗに対する姿勢を表現する上でも使用する。車両部１Ｗの上面をロボット基準面と呼ぶ。
Ｘ１軸:車両部１Ｗの左右方向に平行な軸。
Ｙ１軸:車両部１Ｗの前後方向に平行な軸。
Ｚ１軸:車両部１Ｗの高さ方向に平行な軸。
Ｘ１軸、Ｙ１軸、Ｚ１軸は、互いに直交する。右方をＸ１軸の正の向きとし、前方を１軸の正の向きとし、上方をＺ１軸の正の向きとする。Ｘ１軸およびＹ１軸は、ロボット基準面に平行である。車両部１Ｗの姿勢は、Ｘ１軸、Ｙ１軸およびＺ１軸がＸＹＺ座標系で向く方向により表現する。Ｙ１軸をＸＹ平面に射影した軸（投影Ｙ１軸）とＸ軸がなす角度が、ヨー角である。投影Ｙ１軸がＸＹ平面となす角度が、ピッチ角である。投影Ｙ１軸およびＺ軸を含む平面とＺ１軸とがなす角度が、ロール角である。

目標姿勢入力装置６は、ヒューマノイド部１Ｈを制御するための上体目標姿勢入力部６Ｈと、車両部１Ｗを制御するための車両姿勢入力部６Ｗとを有する。上体目標姿勢入力部６Ｈは、オペレータ３０が手を使用してヒューマノイド部１Ｈに対する目標姿勢を入力する装置である。車両姿勢入力部６Ｗは、オペレータ３０が足を使用して車両部１Ｗを操縦する装置である。目標姿勢は、ヒューマノイド部１Ｈを含むロボット１にとらせる姿勢である。目標姿勢は、ロボット１がとる姿勢の目標となる姿勢として入力される。入力された目標姿勢を表すデータを、目標姿勢５４と呼ぶ。オペレータ５０は、目標姿勢を入力し、入力した目標姿勢を変更できる。

ヒューマノイド部１Ｈは、骨格部１１が関節部１２により回転可能に接続したものである。目標姿勢５４は、各関節部１２が有する各回転軸の回りの回転角度に対する指令値として表現される。各関節部１２が目標姿勢５４で指令される角度をとると、ヒューマノイド部１Ｈは目標姿勢５４をとる。長さが変更可能な部分をヒューマノイド部が有する場合には、長さが変更可能な部分の長さに対する指令値も目標姿勢は含む。ヒューマノイド部１Ｈの構造については、後で説明する。

上体目標姿勢入力部６Ｈは、各関節部１２が有する各回転軸の角度を指定する指令値である目標姿勢５４を入力する装置である。上体目標姿勢入力部６Ｈでは、例えばレバーなどをオペレータ３０が操作することで、目標姿勢５４を入力する。

車両姿勢入力部６Ｗをオペレータ３０が足で操作することで、車両部１Ｗの前進、後進、右回転および左回転の指示を入力する。オペレータ３０が入力する指示に応じて車両部１Ｗが移動する。車両部１Ｗの位置とヨー角が、オペレータ３０が意図するものになると、車両姿勢５６の入力が完了する。車両部１Ｗのピッチ角とロール角は、車両部１Ｗが有するクローラの下に存在する地面や建築物などの形状により決まる。車両部が、ピッチ角およびロール角の少なくとも一方を変更する機構を備えてもよい。その場合には、車両姿勢入力部により、オペレータ３０はピッチ角およびロール角の少なくとも一方を変更する機構に対する指令値も入力する。

右カメラ２Ｒおよび左カメラ２Ｌは、ロボット１の頭部１１Ｆに搭載される。右カメラ２Ｒおよび左カメラ２Ｌは、決められた位置関係を有する。例えば、右カメラ２Ｒおよび左カメラ２Ｌは、頭部１１Ｆの正面側の上部の決められた同じ高さの位置に決められた間隔で配置される。右カメラ２Ｒおよび左カメラ２Ｌは、ロボット１のハンド部１１Ｅが操作対象である物体２０を扱う様子を撮影する。人間が物体２０を扱う際に見えるのと同様な画像が撮影できるように、右カメラ２Ｒおよび左カメラ２Ｌはロボット１の頭部１１Ｆに搭載する。右カメラ２Ｒは、右目画像５１Ｒを撮影する。左カメラ２Ｌは、左目画像５１Ｌを撮影する。右カメラ２Ｒおよび左カメラ２Ｌの位置から見える視野を、ロボット視野と呼ぶ。

右目画像５１Ｒおよび左目画像５１Ｌは、立体視表示装置４により立体的に見えるように表示される。立体的に見えるように表示することを立体視表示するという。立体視表示される右目画像５１Ｒおよび左目画像５１Ｌを、３Ｄ画像５２と呼ぶ。立体視表示装置４は、決められた間隔で配置された２個の接眼レンズを有する。オペレータ３０は、右目および左目で立体視表示装置４の接眼レンズの中を覗き込むことで立体視表示されている３Ｄ画像５２を見ることができる。

立体視表示装置４は、右目画像５１Ｒを表示する右目用スクリーンと、左目画像５１Ｌを表示する左目用スクリーンとを有する。物体２０および周囲環境との距離や物体２０の大きさなどが、オペレータ３０が現場にいる場合に見えるものと同様になるように、右目用スクリーンおよび左目用スクリーンの位置および間隔や接眼レンズなどの光学系は設計されている。つまり、立体視表示装置４における輻輳角は、ロボット１に搭載された２台のカメラである右カメラ２Ｒおよび左カメラ２Ｌが持つ輻輳角と同じである。立体視表示装置４により、オペレータ３０は自然な立体視画像を見ることができる。そのため、オペレータ３０は、物体２０に対する正確な奥行き感を把握できる。

立体視表示装置４は、接眼レンズを有さず、液晶シャッタや偏光眼鏡などを使用するものでもよい。立体視表示装置４は、立体視表示するものであればどのようなものでもよい。

表示装置５は、モデル画像５３を表示する。モデル画像５３は、物体２０を含む周囲環境とロボット１の３次元モデルに基づき、ロボット１および周囲環境を任意の視点から見る画像である。モデル画像５３の視点は、オペレータ３０が入力する情報に基づき決められる。表示装置５には、ロボット１を遠隔制御する上で有用な情報を表示する。ロボット１を遠隔制御する上で有用な情報を、制御参考情報と呼ぶ。表示装置５は、複数個でもよい。

図１に示す例では、オペレータ３０が椅子に座り、オペレータ３０の頭の前方に箱状の立体視表示装置４を設け、立体視表示装置４の右側上方に表示装置５を設けている。立体視表示装置４および表示装置５の配置は、オペレータ３０が頭を移動させることで、どちらかの表示装置を選択して見ることができる位置であればどのような配置でもよい。表示装置５は、大きな画面を有するものや、スクリーンにプロジェクタで投影するものでもよい。表示装置５は、オペレータ３０が頭を移動させることで立体視表示装置４と切り替えて見ることができるように配置されていればよい。

オペレータ３０は、立体視表示装置４の接眼レンズを覗き込むことで物体２０などをロボット視野で立体視できる。オペレータ３０は、頭の位置および向きを変化させることで表示装置５にされたモデル画像５３を見ることができる。

制御装置３は、ロボット１を遠隔制御する。制御装置３は、ＣＰＵ２１、メモリ部２２などを含む電子計算機により実装される。メモリ部２２は、ＣＰＵ２１で実行されるプログラムや処理に使用するデータあるいは処理の結果で得られるデータなどを記憶する。メモリ部２２は、フラッシュメモリのような半導体メモリおよびハードディスクである。メモリ部２２は、揮発性の記憶装置および不揮発性の記憶装置を含む。制御部７も、ＣＰＵ２３、メモリ部２４などを含む電子計算機により実装される。

ロボット１と制御装置３は、通信回線８を介して接続されて、互いに通信する。通信回線８により、制御装置３からロボット１を制御する制御信号が送られる。制御信号は、目標姿勢５４および制御姿勢５５を制御部７に伝える信号である。制御姿勢５５は、目標姿勢５４で表現される姿勢をロボット１がとる過程でロボット１がとる姿勢を表すデータである。制御姿勢５５は、ロボット１を制御する周期において、その周期の終了時点にロボット１がとるべき姿勢を表すデータである。制御姿勢５５は、目標姿勢５４と同じ形式で表現される。目標姿勢５４および制御姿勢５５は、制御装置３で周期的に計算されて、計算されるとすぐに制御部７に送られる。ロボット１からは、右目画像５１Ｒ、左目画像５１Ｌおよびロボット１に搭載されている各種センサが検出したセンサ情報が制御装置３に送られる。右目画像５１Ｒおよび左目画像５１Ｌは、立体視表示装置４にも送られる。

制御部７は、制御装置３と通信するための通信部９を有する。通信回線８は、有線でも無線でもよく、公衆回線でも専用回線でもよい。用途に合わせた通信回線を使用する。ロボット１が物体２０を操作する現場と、オペレータ３０が居る指令所との間の距離は、任意である。距離は、何千ｋｍも離れていてもよいし、１ｍでもよい。制御装置３は、指令所がある建物に設置される。制御装置３を、別の建物に設置してもよい。立体視表示装置４、表示装置５、上体目標姿勢入力部６Ｈおよび車両姿勢入力部６Ｗと制御装置３との間は、ＬＡＮ１０で接続する。遠隔制御とは、遠隔で機械を制御する方法で制御するという意味である。ロボット１などの機械と制御装置３の実際の距離は、遠隔でなくてもよい。

ヒューマノイド部１Ｈは、骨格部１１、関節部１２、アクチュエータ１３、モータ１４を、主に有して構成される。骨格部１１は、ヒューマノイド部１Ｈの各部を構成する部材である。関節部１２は、接続角度を変更可能に骨格部１１を接続する。アクチュエータ１３およびモータ１４は、関節部１２を動かす力を発生させる。

骨格部１１は、胴体部１１Ａ、腕基部１１Ｂ、上腕部１１Ｃ、前腕部１１Ｄ、ハンド部１１Ｅ、頭部１１Ｆ、レッグ部１１Ｇおよび胴体接続部１１Ｈの何れかである。上腕部１１Ｃ、前腕部１１Ｄおよびハンド部１１Ｅは、それぞれ人間の上腕、前腕および手に相当する。ハンド部１１Ｅは、人間の手と同様な形状で５本指である。レッグ部１１Ｇおよび胴体接続部１１Ｈは、人間では下半身に相当する。レッグ部１１Ｇおよび胴体接続部１１Ｈは、車両部１Ｗと胴体部１１Ａの間に直列に存在する。レッグ部１１Ｇは、車両部１Ｗに接続する。胴体接続部１１Ｈは、胴体部１１Ａおよびレッグ部１１Ｇに接続する。レッグ部１１Ｇおよび胴体接続部１１Ｈが動くと、胴体部１１Ａの車両部１Ｗに対する位置および角度が変化する。

腕基部１１Ｂ、上腕部１１Ｃ、前腕部１１Ｄおよびハンド部１１Ｅは、腕部１１Ｊを構成する。胴体部１１Ａの上部の左右に腕基部１１Ｂが接続する。腕基部１１Ｂ、上腕部１１Ｃ、前腕部１１Ｄおよびハンド部１１Ｅは、直列に接続する。腕基部１１Ｂにより、腕部１１Ｊが胴体部１１Ａの側面に垂直な回転軸の回りを回転できる。

関節部１２は、肩関節部１２Ａ、肘関節部１２Ｂ、手首関節部１２Ｃ、首関節部１２Ｄ、レッグ元関節部１２Ｅ、レッグ先関節部１２Ｆおよび胴体関節部１２Ｇの何れかである。肩関節部１２Ａ、肘関節部１２Ｂおよび手首関節部１２Ｃは、それぞれ人間の肩関節、肘関節および手首関節に相当する。首関節部１２Ｄは、頭部１１Ｆを胴体部１１Ａに回転可能に接続する。レッグ元関節部１２Ｅとレッグ先関節部１２Ｆは、それぞれレッグ部１１Ｇの根元側と先端側に存在する関節部１２である。胴体関節部１２Ｇは、胴体部１１Ａを胴体接続部１１Ｈに回転可能に接続する。

図６および図７を参照して、関節部１２について説明する。図６は、ロボット１が有する回転軸を説明する図である。図６(A)に背面図を示し、図６(B)に左側面図を示す。図７は、腕部１１Ｊが有する回転軸を説明する図である。図７には、左の腕部１１Ｊを示す。図７(A)に右側面図を示し、図７(B)に正面図を示す。

レッグ元関節部１２Ｅは、レッグ部１１Ｇを２回転自由度で回転可能に車両部１Ｗに接続する。レッグ元関節部１２Ｅは、ＡＺ１軸とＥＬ１軸の回りに回転可能である。ＡＺ１軸とＥＬ１軸は、同一平面上にあり直交する。ＥＬ１軸とＡＺ１軸の交点を、第１の直交座標系の原点とする。ＥＬ１軸は、Ｘ１Ｙ１平面上に存在する。ＡＺ１軸は、車両部１Ｗの上面（ロボット基準面）に垂直な回転軸である。ロボット基準面は、Ｘ１Ｙ１平面に平行な平面である。ＡＺ１軸は、Ｘ１Ｙ１平面に射影したレッグ部１１Ｇが向く方向を変更する回転軸である。ＡＺ１軸は、Ｚ１軸と平行である。ＥＬ１軸は、レッグ部１１ＧとＸ１Ｙ１平面とがなす角度を変更する回転軸である。ＡＺ１軸を方位回転軸とも呼ぶ。ＥＬ１軸を仰角回転軸とも呼ぶ。

ヒューマノイド部１Ｈの姿勢を表現するため、第２の直交座標系（Ｘ２Ｙ２Ｚ２座標系）と第３の直交座標系（Ｘ３Ｙ３Ｚ３座標系）を定義する。第２の直交座標系は、胴体接続部１１Ｈを基準とする座標系である。第２の直交座標系は、胴体接続部１１Ｈとともに移動する。第３の直交座標系は、胴体部１１Ａを基準とする座標系である。第３の直交座標系は、胴体部１１Ａとともに移動する。

レッグ先関節部１２Ｆは、レッグ部１１Ｇの先端に胴体接続部１１Ｈを２回転自由度で回転可能に接続する。レッグ先関節部１２Ｆは、ＥＬ２軸とＡＺ２軸の回りに回転可能である。ＥＬ２軸とＡＺ２軸は直交する。ＥＬ２軸は、ＥＬ１軸に平行な回転軸である。ＥＬ２軸は、胴体接続部１１Ｈがレッグ部１１Ｇとなす角度を変更する。ＡＺ２軸は、胴体接続部１１Ｈを通る回転軸である。胴体接続部１１ＨがＡＺ２軸の回りに回転すると、胴体接続部１１Ｈ、胴体部１１Ａおよび頭部１１Ｆがレッグ部１１Ｇに対して回転する。ＥＬ２軸を、胴体傾斜回転軸とも呼ぶ。ＡＺ２軸を胴体接続部回転軸とも呼ぶ。頭部１１Ｆの正面は、右カメラ２Ｒおよび左カメラ２Ｌが設けられた面である。基準状態では、ヒューマノイド部１Ｈの各部の方向に関して、以下が成立する。胴体接続部１１Ｈおよび胴体部１１Ａの左右方向は、車両部１Ｗの左右方向に平行である。胴体接続部１１Ｈおよび胴体部１１Ａの前後方向（正面方向）は、車両部１Ｗの前後方向に平行である。頭部１１Ｆの正面方向は、胴体接続部１１Ｈおよび胴体部１１Ａの正面方向に平行である。

胴体接続部１１Ｈを基準とする第２の直交座標系を以下のように定義する。第２の直交座標系により、胴体部１１Ａと胴体接続部１１Ｈとの間の位置関係を表現する。
Ｘ２軸:胴体接続部１１Ｈの左右方向に平行な軸。
Ｙ２軸:胴体接続部１１Ｈの前後方向に平行な軸。
Ｚ２軸:胴体接続部１１Ｈの高さ方向に平行な軸。ＡＺ２軸と一致する。
Ｘ２軸、Ｙ２軸、Ｚ２軸は、互いに直交する。胴体接続部１１Ｈの下端とＡＺ２軸の交点を、第２の直交座標系の原点とする。基準状態では、Ｘ１軸とＸ２軸、Ｙ１軸とＹ２軸、Ｚ１軸とＺ２軸は、それぞれ互いに平行である。基準状態での車両部１Ｗの右方をＸ２軸の正の向きとし、前方をＹ２軸の正の向きとし、上方をＺ２軸の正の向きとする。

胴体関節部１２Ｇは、胴体部１１Ａを胴体接続部１１Ｈに２回転自由度で回転可能に接続する。胴体関節部１２Ｇは、ＸＥＬ軸とＡＺ３軸の回りに回転可能である。ＸＥＬ軸は、ＡＺ２軸およびＡＺ３軸と直交する。ＸＥＬ軸は、胴体接続部１１Ｈが延在する方向と、胴体部１１Ａが延在する方向との間の角度を変更する回転軸である。ＡＺ３軸は、胴体部１１Ａを通る回転軸である。ＡＺ３軸の回りに回転すると、胴体接続部１１Ｈの正面方向に対して、胴体部１１Ａの正面方向が回転する。ＸＥＬ軸を胴体交差回転軸とも呼ぶ。ＡＺ３軸を胴体回転軸とも呼ぶ。

胴体部１１Ａを基準とする第３の直交座標系を、以下のように定義する。第３の直交座標系により、胴体部１１Ａに対する腕部１１Ｊの位置を表現する。
Ｘ３軸:胴体部１１Ａの左右方向に平行な軸。
Ｙ３軸:胴体部１１Ａの前後方向に平行な軸。
Ｚ３軸:胴体部１１Ａの高さ方向に平行な軸。ＡＺ３軸と一致する。
Ｘ３軸、Ｙ３軸、Ｚ３軸は、互いに直交する。第３の直交座標系の原点は、２個の肩関節部１２Ａを通りＡＺ３軸に垂直な平面とＡＺ３軸との交点とする。基準状態での胴体部１１Ａの右方をＸ３軸の正の向きとし、前方をＹ３軸の正の向きとし、上方をＺ３軸の正の向きとする。

首関節部１２Ｄは、頭部１１Ｆを胴体部１１Ａの上面に３回転自由度で回転可能に接続する。首関節部１２Ｄは、ＸＨ軸、ＹＨ軸およびＡＺＨ軸の回りに回転可能である。ＸＨ軸は、Ｘ３軸に平行な回転軸である。ＹＨ軸は、Ｙ３軸に平行な回転軸である。ＡＺＨ軸は、頭部１１Ｆを通る回転軸である。ＸＨ軸、ＹＨ軸およびＡＺＨ軸は、首関節部１２Ｄで交差する。ＡＺＨ軸を、頭部回転軸とも呼ぶ。基準状態では、ＡＺＨ軸はＺ３軸と平行である。

首関節部１２Ｄにより頭部１１ＦがＡＺＨ軸の回りに回転すると、右カメラ２Ｒおよび左カメラ２Ｌの撮影方向は、左右方向に回転する。首関節部１２Ｄにより頭部１１ＦがＸＨ軸の回りに回転すると、右カメラ２Ｒおよび左カメラ２Ｌの撮影方向は、上下方向に変化する。首関節部１２Ｄにより頭部１１ＦがＹＨ軸の回りに回転すると、右カメラ２Ｒおよび左カメラ２Ｌの中心を結ぶ線分が水平でなくなり、右カメラ２Ｒおよび左カメラ２Ｌが存在する高さに差が発生する。

ヒューマノイド部１Ｈ（腕部１１Ｊは除く）が有する回転軸は、以下の９軸である。
ＡＺ１軸：レッグ元関節部１２Ｅでの方位回転軸。
ＥＬ１軸：レッグ元関節部１２Ｅでの仰角回転軸。
ＥＬ２軸：レッグ先関節部１２Ｆでの胴体傾斜回転軸。
ＡＺ２軸：レッグ先関節部１２Ｆでの胴体接続部回転軸。
ＸＥＬ軸：胴体関節部１２Ｇでの胴体交差回転軸。
ＡＺ３軸：胴体関節部１２Ｇでの胴体回転軸。
ＸＨ軸：首関節部１２ＤでのＸ３軸に平行な回転軸。
ＹＨ軸：首関節部１２ＤでのＹ３軸に平行な回転軸。
ＡＺＨ軸：首関節部１２Ｄでの頭部回転軸。

腕部１１Ｊの構造を説明する。腕基部１１Ｂは、胴体部１１Ａに１回転自由度で回転可能に接続する。上腕部１１Ｃは、肩関節部１２Ａにより腕基部１１Ｂに２回転自由度で回転可能に接続する。前腕部１１Ｄは、肘関節部１２Ｂにより上腕部１１Ｃに２回転自由度で回転可能に接続する。ハンド部１１Ｅは、手首関節部１２Ｃにより前腕部１１Ｄに２回転自由度で回転可能に接続する。

図７に示すように、腕部１１Ｊは、以下に示す７個の回転軸を有する。
ＡＺ４軸：腕部１１Ｊを胴体部１１Ａに対して回転させる回転軸。腕基部１１Ｂを通り、腕基部１１Ｂを回転させる回転軸。ＡＺ４軸を、腕基部回転軸と呼ぶ。
ＥＬ４軸：腕基部１１Ｂと上腕部１１Ｃとがなす角度を変更させる回転軸。ＥＬ４軸は、ＡＺ４軸と直交する。
ＡＺ５軸：上腕部１１Ｃを通り、上腕部１１Ｃを回転させる回転軸。ＡＺ５軸は、ＥＬ４軸と直交する。ＡＺ５軸を、上腕部回転軸と呼ぶ。
ＥＬ５軸：上腕部１１Ｃと前腕部１１Ｄとがなす角度を変更させる回転軸。ＥＬ５軸は、ＡＺ５軸と直交する。
ＡＺ６軸：前腕部１１Ｄを通り、前腕部１１Ｄを回転させる回転軸。ＡＺ６軸は、ＥＬ５軸と直交する。ＡＺ６軸を、前腕部回転軸と呼ぶ。
ＥＬ６軸：前腕部１１Ｄとハンド部１１Ｅとがなす角度を、ＡＺ６軸およびＸＥＬ２軸を含む平面（前後方向回転平面）において回転させる回転軸。ＥＬ６軸は、ＡＺ６軸およびＸＥＬ２軸と直交する。
ＸＥＬ２軸：前腕部１１Ｄとハンド部１１Ｅとがなす角度を、ＡＺ６軸およびＥＬ６軸を含む平面（左右方向回転平面）において回転させる回転軸。ＸＥＬ２軸は、ＡＺ６軸およびＥＬ６軸と直交する。

ＡＺ４軸は、腕基部１１Ｂを胴体部１１Ａに対して回転させる回転軸である。ＥＬ４軸とＡＺ５軸は、肩関節部１２Ａにおいて腕基部１１Ｂと上腕部１１Ｃとの接続角度を変更する回転軸である。ＥＬ５軸とＡＺ６軸は、肘関節部１２Ｂにおいて上腕部１１Ｃと前腕部１１Ｄとの接続角度を変更する回転軸である。ＥＬ６軸とＸＥＬ２軸は、手首関節部１２Ｃにおいて前腕部１１Ｄとハンド部１１Ｅとの接続角度を変更する回転軸である。前後方向回転平面および左右方向回転平面は、それらの交線が前腕部１１Ｄを通る互いに直交する２平面である。

手首関節部１２Ｃは、伸縮する２本のアクチュエータ１３により角度が変更される。他の関節部１２は、骨格部１１あるいは関節部１２の内部に配置されたモータ１４により、関節の角度が変更される。他の各関節部１２には、その関節部１２での回転自由度の次数と同じ個数のモータ１４が配置される。

姿勢センサ９２は、各モータ１４（各アクチュエータ１３を伸縮させる力を発生させるモータを含む）の回転角度を、周期的に計測する。計測した各モータ１４の回転角度は、モータごとに決められた換算係数により各関節部１２の角度に変換される。各関節部１２の角度が決まると、ヒューマノイド部１Ｈの姿勢が決まる。各関節部１２の角度を、姿勢データ５７と呼ぶ。姿勢センサ９２は、間接的に姿勢データ５７を計測する。姿勢データ５７は、現在の各関節部１２の角度である。現在の角度は、最新の時点で計測された角度である。姿勢データ５７は、各腕部１１Ｊが有する７個の回転軸の角度情報、腕部１１Ｊを含まないヒューマノイド部１Ｈが有する９個の回転軸の角度情報、およびハンド部１１Ｅが有する指関節部の角度情報を含む。姿勢データ５７を計測する周期を制御周期Ｔｃと呼ぶ。Ｔｃは、例えば０．１秒である。１組の姿勢データ５７により、１個の目標姿勢５４または制御姿勢５５を表現する。車両姿勢５６および姿勢データ５７は、ロボット１の位置および姿勢を表す。車両姿勢５６および姿勢データ５７を、位置姿勢情報と呼ぶ。位置センサ９１および姿勢センサ９２を、位置姿勢情報を検出する位置姿勢センサとよぶ。

長さが変更可能な部分をヒューマノイド部が有する場合には、長さが変更可能な部分の長さを計測するセンサも姿勢センサに含まれる。姿勢データには、長さが変更可能な部分の長さの計測値も含まれる。

温度センサ９３により、各モータ１４の温度を計測する。各モータ１４の温度のデータを、モータ温度５８と呼ぶ。各モータ１４の温度を計測することで、各モータ１４の過熱を検出する。過熱が検出されるモータ１４は停止させるなどして、各モータ１４が損壊することを防止する。

ハンド部１１Ｅの指先および掌部には、近くに存在する物体までの距離を計測する距離センサ９４を設ける。距離センサ９４が計測する至近の物体までの距離を表すデータを、物体距離５９と呼ぶ。物体距離５９も利用して、ハンド部１１Ｅが物体２０の近くに存在するかどうかを制御部７が判断する。

感圧センサ９５は、ハンド部１１Ｅの指先が物体に接触しているかどうかを検出する。感圧センサ９５は、指先が物体に接触している場合には物体と指先との間に働く接触力の大きさも計測する。触覚データ６０は、感圧センサ９５が検出する接触の有無と接触力を含むデータである。手首関節部１２Ｄには、力覚センサ９６を備える。力覚センサ９６は、手首関節部１２Ｄに作用する力を計測する。力覚センサ９６が計測する力のデータを、作用力データ６１と呼ぶ。

胴体部１１Ａ、上腕部１１Ｃ、前腕部１１Ｄ、ハンド部１１Ｅおよび胴体接続部１１Ｈには、加速度センサ９７を備える。加速度センサ９７は、加速度を検出する。検出された加速度を表すデータを加速度データ６２と呼ぶ。加速度データ６２を積分して各部の速度を求める。また、加速度センサ９７は、ロボット１の各部が振動しているかどうか、ロボット１に外部から力が加えられているかどうかを検出するために設ける。

車両姿勢５６、姿勢データ５７、モータ温度５８、物体距離５９、触覚データ６０、作用力データ６１および加速度データ６２は、制御部７に入力される。制御部７は、これらのデータを記憶する記憶部１５を有する。姿勢データ５７、モータ温度５８、物体距離５９、触覚データ６０、作用力データ６１および加速度データ６２は、データの種類ごとに決められた期間の間は、記憶部１５に記憶される。記憶部１５には、制御装置３から送信される目標姿勢５４および制御姿勢５５も記憶される。目標姿勢５４および制御姿勢５５のそれぞれは、各関節部１２が有する各回転軸の回りの回転角度に対する指令値として表現される。

制御部７は、ロボット１を制御する上で入力されたデータを使用する。制御部７は、周期的に入力される姿勢データ５７が制御姿勢５５と一致するようにアクチュエータ１３およびモータ１４を制御する。制御部７による制御は、フィードバック制御でもよいし、低次自律運動判断による制御でもよい。

制御部７は、通信部９を介して各種センサが計測したデータなどを制御装置３に送信する。通信部９は、通信パケットを送受信する機能だけでなく、記憶部１５を参照して送信するデータを生成する機能、および受信したデータを記憶部１５の該当する領域に格納する機能も有する。

右カメラ２Ｒおよび左カメラ２Ｌは、ヒューマノイド部１Ｈの頭部１１Ｆに装着される。右カメラ２Ｒおよび左カメラ２Ｌは、頭部１１Ｆが向く方向を変更することで、画像を撮影する方向である撮影方向を変更できる。頭部１１Ｆが向く方向が変化しても、右カメラ２Ｒおよび左カメラ２Ｌの間の位置関係は変化しない。右カメラ２Ｒおよび左カメラ２Ｌは、絞りや拡大倍率などを含む撮影条件データ６３を、撮影した画像とともに制御部７に出力する。制御部７は、通信部９を介して右目画像５１Ｒ、左目画像５１Ｌと撮影条件データ６３を制御装置３に送信する。

制御部７を介さないで、右目画像５１Ｒおよび左目画像５１Ｌを制御装置３に送信してもよい。制御部７は、撮影時刻と対応付けた撮影条件データ６３を制御装置に送信する。制御装置３では、撮影時刻により右目画像５１Ｒおよび左目画像５１Ｌと撮影条件データ６３とを関連付ける。

右目画像５１Ｒ、左目画像５１Ｌおよび撮影条件データ６３は、ロボット１の制御部７とは別の装置が保存し、制御装置３に送信してもよい。記憶部１５は、制御部７とは別に設けてもよい。

制御装置３は、通信部３１、構造データ記憶部３２、状態データ記憶部３３、ロボットモデル生成部３４、環境モデル生成部３５、モデル統合部３６、視点データ生成部３７、モデル画像生成部３８、制御参考情報生成部３９、目標姿勢生成部４０、制御姿勢生成部４１および異常検出部４２を主に有して構成される。構造データ記憶部３２は、ロボット１の構造を表すロボット構造データ８１などを記憶する。状態データ記憶部３３は、姿勢データ５７などの変化するロボット１に関するデータを記憶する。

通信部３１は、ロボット１などと通信する。通信部３１は、通信パケットを送受信する機能だけでなく、構造データ記憶部３２、状態データ記憶部３３を参照して送信するデータを生成する機能、および受信したデータを状態データ記憶部３３の該当する領域に格納する機能も有する。

構造データ記憶部３２は、ロボット構造データ８１、カメラ構造データ８２およびセンサ構造データ８３を記憶する。ロボット構造データ８１は、ロボット１が動いても変化しないデータである。ロボット構造データ８１は、例えば骨格部１１と関節部１２の接続関係や、骨格部１１の寸法や関節部１２の回転可能角度、骨格部１１の外形を表現する部分３Ｄモデルなどである。レッグ部１１Ｇ、上腕部１１Ｃや前腕部１１Ｄなどの長さ、２個の肩関節部１２Ａの間隔なども、ロボット構造データ８１である。ロボット構造データ８１は、マニピュレータであるロボット１の構造を表すマニピュレータ構造データである。

カメラ構造データ８２は、右カメラ２Ｒおよび左カメラ２Ｌの構造を表すデータである。カメラ構造データ８２は、右カメラ２Ｒおよび左カメラ２Ｌに共通なデータとして、撮影できる画像の縦横のサイズ、視野角、拡大倍率の可能範囲などを含む。カメラ構造データ８２は、右カメラ２Ｒおよび左カメラ２Ｌの首関節部１２Ｄに対する相対位置、右カメラ２Ｒおよび左カメラ２Ｌの間隔、右カメラ２Ｒおよび左カメラ２Ｌの正面方向を含む。右カメラ２Ｒおよび左カメラ２Ｌの正面方向は、それぞれの視野の中心が頭部１１Ｆに対して向く方向である。右カメラ２Ｒおよび左カメラ２Ｌの首関節部１２Ｄに対する相対位置と正面方向は、ロボット構造データ８１として記憶してもよい。カメラの撮影方向は、カメラの正面方向を現場座標系（ＸＹＺ座標系）で表現した方向である。

センサ構造データ８３は、各種センサの設置位置、センサの種類、計測されるデータが送信される際のデータの格納位置などデータである。

状態データ記憶部３３は、車両姿勢５６、姿勢データ５７、モータ温度５８、物体距離５９、触覚データ６０、作用力データ６１、加速度データ６２、撮影条件データ６３、ロボット３次元モデル６４、カメラ姿勢６５、カメラ点群生データ６６、カメラ点群データ６７、周囲点群データ６８、統合３次元モデル６９および視点データ７０を記憶する。

ロボット３次元モデル６４は、右カメラ２Ｒおよび左カメラ２Ｌを含むロボット１の３次元モデルである。ロボット３次元モデル６４は、現場座標系（ＸＹＺ座標系）で表現される。ロボット３次元モデル６４は、ロボットモデル生成部３４により生成される。ロボット３次元モデル６４は、カメラ姿勢６５を含む。カメラ姿勢６５は、右カメラ２Ｒおよび左カメラ２Ｌの現場座標系における位置および撮影方向を表すデータである。ロボット３次元モデル６４は、マニピュレータであるロボット１の３次元モデルである。カメラ姿勢６５は、右カメラ２Ｒおよび左カメラ２Ｌの３次元モデルである。

カメラ点群生データ６６は、右目画像５１Ｒおよび左目画像５１Ｌを画像処理して得られる物体２０を含む周囲環境の事物を表す点群データである。点群データは、事物を表す点の座標値の集合である。カメラ点群生データ６６は、右カメラ２Ｒおよび左カメラ２Ｌのそれぞれを基準とする相対的な位置として表現される。カメラ点群データ６７は、カメラ点群生データ６６を座標変換して現場座標系での座標値として表現した点群データである。周囲点群データ６８は、異なるカメラ姿勢６５でのカメラ点群データ６７を統合して生成された点群データである。周囲点群データ６８は、周囲環境の３次元モデルである周囲環境モデルである。異なるカメラ姿勢６５とは、右カメラ２Ｒおよび左カメラ２Ｌの少なくとも一方について、その位置および姿勢の少なくとも一方が異なることである。

統合３次元モデル６９は、ロボット３次元モデル６４と周囲点群データ６８とを統合した３次元モデルである。視点データ７０は、モデル画像５３において物体２０およびロボット１を見る視点の位置と視線方向を表すデータである。

ロボットモデル生成部３４は、入力される最新の車両姿勢５６および姿勢データ５７、ロボット構造データ８１およびカメラ構造データ８２を参照してロボット３次元モデル６４を生成する。ロボット３次元モデル６４は、マニピュレータの３次元モデルであるマニピュレータモデルである。ロボットモデル生成部３４は、マニピュレータモデルを生成するマニピュレータモデル生成部である。ロボット１の各関節部の角度を表現する姿勢データ５７を、ロボット構造データ８１で決まる式に代入して、ロボットモデル生成部３４は、第１の直交座標系（Ｘ１Ｙ１Ｚ１座標系）における各骨格部１１の姿勢を決める。各骨格部１１の姿勢を決める方法は、例えば特許文献２に開示されている方法を使用する。さらに、各骨格部１１の外形を表現する部分３Ｄモデルを各骨格部１１の姿勢に応じて移動および回転させる。移動および回転した各骨格部１１の部分３Ｄモデルをまとめたものがロボット３次元モデルである。この段階のロボット３次元モデルは、第１の直交座標系で表現される。ロボットモデル生成部３４は、車両姿勢５６を使用して、第１の直交座標系におけるロボット３次元モデルを現場座標系でのロボット３次元モデル６４に変換する。ロボット３次元モデル６４は、カメラ姿勢６５も含む。カメラ姿勢６５は、ロボットモデル生成部３４とは別の機能モジュールが生成してもよい。その場合は、ロボットモデル生成部は、カメラ構造データ８２を参照しない。

環境モデル生成部３５は、周囲環境モデルである周囲点群データ６８を生成する。環境モデル生成部３５は、画像処理部４３、カメラ点群生データ生成部４４、カメラ座標変換部４５およびカメラ点群整合部４６を有する。画像処理部４３は、右カメラ２Ｒおよび左カメラ２Ｌの位置と撮影方向に基づき、右目画像５１Ｒおよび左目画像５１Ｌを画像処理する。画像処理部４３は、右目画像５１Ｒおよび左目画像５１Ｌを画像処理して、物体２０を含む周囲環境の事物の特徴点を抽出する。特徴点とは、事物の境界線や特徴的な形状を表す点である。なお、画像によっては、特徴点を抽出できない場合がある。例えば、事物の色と背景の色がほとんど同じで、事物の境界線が画像から判断できない場合、事物の表面形状が平坦で特徴点が検出できない場合などである。特徴点を抽出できない場合は、カメラ点群生データ６６は生成されない。

カメラ点群生データ生成部４４は、右目画像５１Ｒおよび左目画像５１Ｌの両方で抽出された特徴点の位置データを生成する。その原理を簡単に説明すると、以下のようになる。１個の画像に表示される特徴点は、その画像を撮影したカメラの位置を通る３次元空間での直線上に存在する。特徴点が存在する直線の方向は、画像内での特徴点の画素位置と撮影方向から決まる。同時または特徴点を有する物体が静止している期間に別の視点から撮影した画像を組み合わせると、特徴点が複数の直線の交点に存在することから、特徴点の２台のカメラの位置に対する相対的な位置を決めることができる。

カメラ点群生データ生成部４４は、右目画像５１Ｒおよび左目画像５１Ｌという視差を有して同時に撮影された２個の画像から各特徴点の３次元空間での位置を決める。右目画像５１Ｒから抽出された特徴点の画像内での画素位置は、画素位置から決まる３次元空間での直線上に特徴点が存在することを意味する。この直線は、右カメラ２Ｒの位置を通り、画像内での画素位置に対応する３次元空間での方向に存在する直線である。例えば、画像の中央の画素は、撮影方向に延在する直線になる。上下方向の中央で左右方向の両端の画素は、撮影方向に対して視野角の半分だけ左右方向に向いた方向に延在する直線になる。左目画像５１Ｌでも同様である。左目画像５１Ｌから抽出された特徴点の画素位置は、左カメラ２Ｌの位置を通りその画素位置に対応する３次元空間での方向を表す直線上に、特徴点が存在することを意味する。右目画像５１Ｒおよび左目画像５１Ｌの両方で抽出された特徴点は、右目画像５１Ｒから決まる３次元空間での直線と、左目画像５１Ｌから決まる３次元空間での直線の交点に存在することになる。カメラ点群生データ生成部４４は、各特徴点に対して、２本の３次元空間での直線の交点を求め、その交点の位置データをその特徴点の位置データとする。すべての特徴点の位置データをまとめたものが、カメラ点群生データ６６である。

カメラ点群生データ６６は、右カメラ２Ｒおよび左カメラ２Ｌに対する相対的な位置データとして表現される。カメラ座標変換部４５は、カメラ姿勢６５を使用して各特徴点のカメラ点群生データ６６をカメラ点群データ６７に変換する。カメラ点群データ６７は、現場座標系での位置データである。同じカメラ姿勢６５での異なる時点での画像から抽出した特徴点の画素位置の平均を使用して、カメラ点群データ６７を生成してもよい。カメラ点群整合部４６は、異なるカメラ姿勢６５での同じ特徴点に対するカメラ点群データ６７の平均をとるなどして、周囲点群データ６８を生成する。周囲点群データ６８は、物体２０を含む周囲環境の事物の３次元モデルである周囲環境モデルである。環境モデル生成部３５は、画像処理により周囲環境モデルを生成する画像処理モデル生成部である。

周囲点群データ６８を生成した後に、新たなカメラ点群データ６７が生成される場合は、カメラ点群整合部４６は、新たなカメラ点群データ６７により周囲点群データ６８を修正する。新たなカメラ点群データ６７で事物の位置が変化している場合は、その事物に関する周囲点群データ６８での点群データは、新たな位置に移動させる。

モデル統合部３６は、ロボット３次元モデル６４と周囲点群データ６８とを統合して統合３次元モデル６９を生成する。

視点データ生成部３７は、オペレータ３０が入力する情報に基づき視点データ７０を生成する。視点データ７０は、モデル画像５３における視点の位置と視線方向を表すデータである。視点データ７０の初期値は、例えば右カメラ２Ｒの視点および撮影方向とする。視点データ７０に関する情報をオペレータ３０が、例えばキーボード、マウスやポインティングデバイスおよびジョイスティックなどの入力装置により入力する。例えば、表示装置５に表示されたモデル画像５３に対して、入力装置を使用してオペレータ３０が左右上下への視点の移動の指示を入力する。あるいは、視点までの距離の増減をオペレータ３０が入力してもよい。視点データ生成部３７は、オペレータ３０が入力した移動方向に存在する事物が見えるように視点が移動した視点データ７０を生成する。例えば、オペレータ３０がモデル画像５３を下に移動させる指示を入力する場合は、視点位置を高くし、視線方向を下向きに変更する。例えば、オペレータ３０が右へ移動させる指示を入力する場合は、視点を上から見て反時計回りに回転した位置に移動させ、視線方向も反時計回りに回転させて変更する。視点の位置と視線方向をそれぞれ独立にオペレータ３０が変更できるようにしてもよい。

モデル画像生成部３８は、統合３次元モデル６９および視点データ７０に基づいてモデル画像５３を生成する。モデル画像生成部３８は、視点データ７０で表される位置にカメラを設置し、視点データ７０で表される視線方向を撮影方向として統合３次元モデル６９を撮影する場合に撮影される画像をモデル画像５３として生成する。生成されたモデル画像５３は、表示装置５に表示される。複数の視点データ７０のそれぞれに対してモデル画像５３を生成して、複数の視点からのモデル画像５３を同時に表示装置５に表示するようにしてもよい。モデル画像５３は、オペレータ３０が指定した視点から統合３次元モデル６９を見る画像である。表示装置５は、モデル画像５３を表示するモデル画像表示装置である。

制御参考情報生成部３９は、表示装置５に表示する制御参考情報を生成する。制御参考情報は、ロボット１に搭載されている各種センサからの情報や、ロボット１の現在の姿勢あるいは目標姿勢に関するデータなどである。例えば、物体２０や周囲環境とロボット１との距離情報や、ロボット１の位置および姿勢やその変化速度に関する情報、各関節部での現在角度や各関節部を駆動するモータの現在温度、ロボット１の各部の加速度と振動の大きさ、各関節部の目標角度などが、制御参考情報である。制御参考情報生成部３９は、情報を取捨選択および加工して制御参考情報を生成する。制御参考情報生成部３９は、有用度が高くオペレータ３０が視認しやすくなるように制御参考情報を生成する。

目標姿勢生成部４０は、上体目標姿勢入力部６Ｈおよび車両姿勢入力部６Ｗに入力されるオペレータ３０の手や足の動きから目標姿勢５４および車両姿勢５６を生成する。目標姿勢５４は、最終的にロボット１がとる姿勢を表現するデータである。制御姿勢生成部４１は、目標姿勢５４で表現される姿勢をロボット１がとるように制御する上で使用する制御姿勢５５を生成する。制御姿勢５５は、決められた時間後にロボット１の各関節がとるべき角度として表現される。制御姿勢５５は、時間の経過に応じて修正される。目標姿勢５４が変更された際にも、制御姿勢５５は修正される。制御姿勢生成部４１は、制御部７が有してもよい。制御姿勢生成部４１は、動作指示からロボット１を制御する制御信号を生成する制御信号生成部である。制御姿勢とは異なる方式で制御信号を生成してもよい。
動作指示からロボット１を制御する制御信号を生成する制御信号生成部であれば、どのようなものでもよい。

異常検出部４２は、モータ温度５８、作用力データ６１および加速度データ６２が入力されて、閾値と比較するなどしてロボット１に異常が発生しているかどうかを検出する。異常検出部４２が異常を検出した場合は、制御装置３はロボット１を停止させる。

動作を説明する。図８は、実施の形態１に係る遠隔制御マニピュレータシステムでロボットを遠隔制御する手順を説明するフローチャートである。ここでは、ロボット１が物体２０を片方のハンド部１１Ｅで持って移動する場合で説明する。

ステップＳ０１で、ロボット構造データ８１、カメラ構造データ８２およびセンサ構造データ８３を、構造データ記憶部３２に記憶させる。

ステップＳ０２で、テストパターンを撮影することで右カメラ２Ｒおよび左カメラ２Ｌを校正する。テストパターンは、例えば決められた大きさおよび形状の点を決められた間隔で配置したパターンである。右カメラ２Ｒまたは左カメラ２Ｌである被校正カメラの正面の決められた距離の位置にテストパターンを配置して、被校正カメラがテストパターンを撮影する。撮影する画像が適正になるように、被校正カメラを校正する。校正の対象はレンズの収差などである。その後、右カメラ２Ｒおよび左カメラ２Ｌで同時にテストパターンを撮影する。右カメラ２Ｒおよび左カメラ２Ｌが撮影する画像が適正な関係になるように、２台のカメラの間隔や設置角度を調整する。右カメラ２Ｒおよび左カメラ２Ｌで個別にテストパターンを撮影する手順を省略してもよい。

ステップＳ０３で、右カメラ２Ｒおよび左カメラ２Ｌが計測する距離を実際の距離と一致させる補正パラメータを求める。ロボット１に校正用の姿勢をとらせて、右カメラ２Ｒおよび左カメラ２Ｌから決められた距離の位置に決められた長さの物体（定規物体と呼ぶ）を配置して、定規物体を撮影する。撮影した画像から定規物体までの距離と定規物体の長さを計測する。何点かの距離で定規物体を撮影して計測する。計測した距離および長さが実際の距離および長さと一致するように、乗算する換算係数と加算するオフセット値を求める。換算係数とオフセット値が、補正パラメータである。

ステップＳ０４で、物体２０とその周囲環境の３次元モデルである周囲点群データ６８を最初に生成する。最初に生成することを、初期生成するという。Ｓ０４での動作は、図９に示すフローチャートで説明する。ステップＳ０５で、物体２０を把持する位置にロボット１を移動させる。ステップＳ０６で、物体２０とその周囲を撮影しながらヒューマノイド部１Ｈを動かして物体２０を片方のハンド部１１Ｅで把持させる。Ｓ０６での動作は、図１０に示すフローチャートで説明する。ステップＳ０７で、片方のハンド部１１Ｅで物体２０を把持した状態でロボット１を移動させる。移動後に、ハンド部１１Ｅを操作して物体２０を離す。
Ｓ０４を省略して、周囲点群データ６８を初期生成しなくてもよい。その場合には、ロボット１が物体２０を扱う際に周囲点群データ６８を生成する。

図９を参照して、物体２０を含む周囲環境の３次元モデルである周囲点群データ６８を生成する手順を説明する。Ｓ１１で、ロボット１を物体２０の近くに移動させる。ステップＳ１２で、位置センサ９１が車両姿勢５６を計測する。ステップＳ１３で、姿勢センサ９２が姿勢データ５７を計測し、車両姿勢５６および姿勢データ５７に基づきカメラ姿勢６５を含むロボット３次元モデル６４を生成する。ステップＳ１４で、右カメラ２Ｒおよび左カメラ２Ｌが物体２０を含む画像を撮影し、撮影した画像から特徴点を抽出しカメラ点群生データ６６を生成する。カメラ点群生データ６６およびカメラ姿勢６５を使用してカメラ点群データ６７を生成し、周囲点群データ６８を生成または修正する。ステップＳ１５で、必要十分な範囲および精度で周囲点群データ６８が生成できたかチェックする。
必要十分な範囲および精度で周囲点群データ６８が生成できている（Ｓ１５でＹＥＳ）場合は、処理を終了する。

必要十分な範囲および精度で周囲点群データ６８が生成できていない（Ｓ１５でＮＯ）場合は、ステップＳ１６で別の姿勢をとるかチェックする。別の姿勢をとる（Ｓ１６でＹＥＳ）場合は、ステップＳ１７でヒューマノイド部１Ｈに別の姿勢をとらせる。Ｓ１７の実行後にＳ１３へ戻る。別の姿勢をとらない（Ｓ１６でＮＯ）場合は、ステップＳ１８でロボット１を別の位置に移動させる。Ｓ１８の実行後にＳ１２へ戻る。

図１０を参照して、ヒューマノイド部１Ｈが物体２０を把持する際の手順を説明する。なお、ヒューマノイド部１Ｈが物体２０を把持する際には、車両部１Ｗは停止している。ステップＳ３１で、姿勢センサ９２が姿勢データ５７を計測する。ステップＳ３２で、車両姿勢５６および姿勢データ５７からカメラ姿勢６５を含むロボット３次元モデル６４を生成する。ステップＳ３３で、右カメラ２Ｒおよび左カメラ２Ｌが物体２０の画像を撮影する。ステップＳ３４で、立体視表示装置４が３Ｄ画像５２を立体視表示する。Ｓ３４と並行してステップＳ３５で、右カメラ２Ｒおよび左カメラ２Ｌの両方の視野内に物体２０が存在するかチェックする。

ハンド部１１Ｅを物体２０に近づける際に、ヒューマノイド部１Ｈにより物体２０が隠れる場合や、物体２０が存在する方向に頭部１１Ｆを向けることが難しい場合に、右カメラ２Ｒおよび左カメラ２Ｌの両方の視野内に物体２０が存在させることができなくなる。

右カメラ２Ｒおよび左カメラ２Ｌの両方の視野内に物体２０が存在する（Ｓ３５でＹＥＳ）場合は、ステップＳ３６で、撮影した画像からカメラ点群データ６７を生成する。ステップＳ３７で、カメラ点群データ６７を生成できたかチェックする。カメラ点群データ６７を生成できている（Ｓ３７でＹＥＳ）場合は、ステップＳ３８でカメラ点群データ６７により周囲点群データ６８を修正する。ステップＳ３９で、周囲点群データ６８とロボット３次元モデル６４を統合して統合３次元モデル６９を生成する。ステップＳ４０で、統合３次元モデル６９からモデル画像５３を生成する。ステップＳ４１で、表示装置５がモデル画像５３を表示する。

右カメラ２Ｒおよび左カメラ２Ｌの何れか少なくとも一方の視野内に物体２０が存在しない（Ｓ３５でＮＯ）場合、あるいはカメラ点群データ６７を生成できていない（Ｓ３７でＮＯ）場合は、Ｓ３９に進む。この場合は、Ｓ３９では、周囲点群データ６８は、保存されているものを使用する。

ステップＳ４２で、オペレータ３０が立体視表示装置４に立体視表示される３Ｄ画像５２または表示装置５に表示されたモデル画像５３を見る。ステップＳ４３で、オペレータ３０がヒューマノイド部１Ｈを動かす。このように、オペレータ３０は、物体２０とロボット１のハンド部１１Ｅの相対的な位置関係を確認しながら、ヒューマノイド部１Ｈを操作する。

ステップＳ４４で、ハンド部１１Ｅの感圧センサ９５の出力(触覚データ６０)が接触ありかチェックする。感圧センサ９５の出力が接触ありであることは、ハンド部１１Ｅが物体に接触していることを意味する。感圧センサ９５の出力が接触ありである（Ｓ４４でＹＥＳ）場合は、処理を終了する。以後は、感圧センサ９５の出力に基づき、ハンド部１１Ｅが物体２０を適切に把持するように制御する。感圧センサ９５の出力が接触ありではない（Ｓ４４でＮＯ）場合は、ステップＳ４５で、決められた制御周期Ｔｃが経過したかチェックする。Ｔｃが経過している（Ｓ４５でＹＥＳ）場合は、Ｓ３１へ戻る。Ｔｃが経過していない（Ｓ４５でＮＯ）場合は、Ｓ３３へ戻る。

遠隔制御マニピュレータシステム１００では、オペレータ３０は、自分の頭を移動させることで、立体視表示された３Ｄ画像５２およびモデル画像５３を自由に切り替えて見ることができる。モデル画像５３は、オペレータ３０が指定した視点からロボット１および物体２０を見る画像である。遠隔制御マニピュレータシステム１００では、物体２０とハンド部１１Ｅとの位置関係を測定し、測定した位置関係を表すモデル画像５３を表示する。オペレータ３０は、３Ｄ画像５２およびモデル画像５３の適切な方である画像または両方の画像を確認する。確認の結果により、ハンド部１１Ｅを含むヒューマノイド部１Ｈの現在状態を把握し、ヒューマノイド部１Ｈを遠隔制御する。オペレータ３０は、現在状態の確認とヒューマノイド部１Ｈを動かす操作を物体２０とハンド部１１Ｅが接触するまで繰り返す。そうすることで、オペレータ３０は、ロボット１を遠隔制御して、対象物を押す、引く、把持する等の作業を、従来よりも正確に、かつ容易に実施できる。

モデル画像５３は、任意の視点から見ることができる。さらに、複数の異なる視点から見るモデル画像５３を同時にあるいは切り替えて表示装置５に表示させ、オペレータ３０は複数の異なる視点から物体２０および周囲の環境を把握できる。表示装置５に例えば２個のモデル画像５３を同時に表示させるようにし、１個の画像は同じ視点からのモデル画像５３を表示させ、他の画像は複数の視点から見るモデル画像５３を切り替えて表示させてもよい。画像、文字あるいはシンボルなどで表現される制御参考情報をオペレータは確認できる。その結果、ロボット１と物体２０との相対的な位置関係を、オペレータ３０は従来よりも正確かつ容易に把握できる。モデル画像５３を表示しない場合には位置関係の把握が難しい状況でも、現在のロボット１と物体２０との位置関係を、オペレータ３０が正確に把握することができ、精密な操作をすることが可能となる。ロボット１と物体２０との位置関係の把握が難しい状況は、例えば、ロボット１の腕部１１Ｊなどが物体２０を隠す場合、ロボット１と物体２０との間に障害物がある場合、頭部１１Ｆを物体２０に向けた姿勢ではハンド部１１Ｅが物体２０に届かない位置にある場合などである。

オペレータ３０は、自然な３Ｄ画像５２を見ることができ、ロボット１の現在の姿勢、および物体２０とその周囲環境の関係性を確認しながらロボットを操作することが可能となる。そのため、オペレータ３０は、より精密かつ正確に物体２０を扱う作業を実施できる。

クローラ移動ロボットではなく、２本足で移動する人型ロボット、上体だけのロボット、片腕のロボットアーム、ロボットハンド、産業用マニピュレータなどに対しても、本開示は適用できる。物体を扱う機械や装置であれば、本開示は適用できる。物体を扱う機械や装置のことをマニピュレータと呼ぶ。物体とは、ロボットが持つ道具、道具で処理される物体、道具を使用しないで操作される物体などである。道具としては、例えばドライバーやペンチなどの工具、包丁やメスなどの刃物、鋏や定規や筆記具などの文具、大工道具、清掃用具、パソコンなどの情報機器、建物のドアや窓、荷物を運搬するための台車など、人を移動させる車椅子や移動式ベッドなどが含まれる。物体は、液体、固体、気体の何れでもよい。人や動物や植物なども物体に含む。

本開示に係る遠隔制御マニピュレータシステムによれば、遠隔制御マニピュレータの作業操作性能を向上させる。遠隔制御するロボットあるいはマニピュレータを使用することで、危険な環境、あるいは人間には過酷なあるいは快適でない環境での作業、重い物体の操作などの重労働作業を、人間の負荷を軽減して実施できる。遠隔制御するロボットあるいはマニピュレータは、災害対応等に携わる作業者を支援できる。また、労働力が不足している産業分野でも、遠隔制御マニピュレータを使用できる。高齢化社会で重要性が増すロボットによる介護の支援などにも、遠隔制御マニピュレータを利用できる。

マニピュレータ遠隔制御システムは、災害救助時や一般産業の工場や工事現場において、遠隔で離れた地点から画像を見ながら遠隔作業機械またはロボットが有するハンドによる把持操作をする場合が想定される。しかし、実運用時は環境や把持対象物の事前学習やモデル化は困難である。本開示の遠隔制御マニピュレータの視覚ベース操作システムであれば、操作者が周囲環境や作業状況を視覚ベースの情報から判断でき、遠隔制御マニピュレータの操作信頼度の向上が可能になると期待される。

目標姿勢入力装置としては、操縦桿、ステアリングホイール(ハンドル)、レバー、ジョイスティック、ボタン、つまみ、ペダルなどで目標姿勢および車両姿勢を入力するものでもよい。オペレータの動作をカメラで撮影して、撮影した画像から目標姿勢を読取る方式を使用してもよい。オペレータの動きあるいは筋肉の緊張度などを読取るセンサをオペレータの体に装着した目標姿勢入力装置を使用してもよい。マニピュレータが動くまたは静止する動作を入力できるものであれば、目標姿勢入力部はどのようなものでもよい。
以上のことは、他の実施の形態にもあてはまる。

実施の形態２．
実施の形態２は、ロボットが測距ライダを備え、測距ライダが計測する点群データから周囲環境の３次元モデルを生成する機能を遠隔制御マニピュレータシステムに持たせた場合である。図１１から図１３を参照して、実施の形態２に係る遠隔制御マニピュレータシステムの構成を説明する。図１１は、実施の形態２に係る遠隔制御マニピュレータシステムで使用されるロボットのヒューマノイド部の正面図である。図１２は、実施の形態２に係る遠隔制御マニピュレータシステムで使用されるロボットのヒューマノイド部の左側面図である。図１３は、実施の形態２に係る遠隔制御マニピュレータシステムの機能構成を示すブロック図である。

図１１および図１２を参照して、ロボット１Ａが図４および図５に示すロボット１とは異なる点を説明する。ロボット１Ａが有するヒューマノイド部１ＨＡは、右の前腕部１１Ｄに測距ライダ１６Ｒを有し、左の前腕部１１Ｄに測距ライダ１６Ｌを有し、胴体部１１Ａの前面に測距ライダ１６Ｂを有する。測距ライダ１６Ｒは、右の前腕部１１Ｄの手首関節部１２Ｃに近い正面側の部分に設ける。前腕部１１Ｄの正面側とは、腕部１１Ｊを下に垂らす基準状態においてヒューマノイド部１ＨＡの正面に向く側である。測距ライダ１６Ｌは、左の前腕部１１Ｄの手首関節部１２Ｃに近い正面側の部分に設ける。測距ライダ１６Ｒ、１６Ｌは、前腕部１１Ｄの内側にレーザ光を放射するように前腕部１１Ｄに搭載される。前腕部１１Ｄの内側は、基準状態での前腕部１１Ｄがロボット１Ａの中心線に向う側である。ロボット１Ａの中心線は、基準状態でのロボット１Ａを正面から見る際に高さ方向に延在する。測距ライダ１６Ｂは、胴体部１１Ａの正面の側に放射するように設けられる。なお、例えば測距ライダ１６Ｒの１個だけをヒューマノイド部が有するようにしてもよい。

測距ライダ１６Ｒ、１６Ｌ、１６Ｂは、スキャン（走査）しながらレーザ光を放射し、レーザ光を放射してから物体に反射されてきた反射レーザ光が受信されるまでの時間に基づき、レーザ光を放射した方向に存在する事物までの距離を計測する。測距ライダ１６Ｒ、１６Ｌ、１６Ｂは、それぞれライダ点群生データ７２を生成する。測距ライダがレーザ光をスキャンする範囲の中心になる方向を、測距ライダの放射方向と呼ぶ。測距ライダの放射方向を測距方向とも呼ぶ。ライダ点群生データ７２は、測距ライダ１６Ｒ、１６Ｌ、１６Ｂごとに決まる放射方向に対する角度ごとに、その角度に存在する事物までの距離を表現するデータの集合である。

ロボット１Ａが有する制御部７Ａは、測距ライダ１６Ｒ、１６Ｌ、１６Ｂを動作させるかどうかも制御する。制御部７Ａが有する記憶部１５Ａは、測距ライダ１６Ｒ、１６Ｌ、１６Ｂが計測するライダ点群生データ７２も記憶する。制御部７を変更しないで、測距ライダ１６Ｒ、１６Ｌ、１６Ｂを動作させるかどうかを制御装置３Ａが制御部７を介さないで制御し、ライダ点群生データ７２が制御部７を介さないで制御装置３Ａに送信されてもよい。

遠隔制御マニピュレータシステム１００Ａの機能構成を示す図１３について、実施の形態１の場合の図２とは異なる点を説明する。制御装置３Ａは、構造データ記憶部３２Ａ、状態データ記憶部３３Ａ、ロボットモデル生成部３４Ａおよび環境モデル生成部３５Ａを変更している。構造データ記憶部３２Ａは、ライダ構造データ８４も記憶する。ライダ構造データ８４は、測距ライダ１６Ｒ、１６Ｌ、１６Ｂのそれぞれの放射方向、スキャン角度範囲を記憶する。放射方向は、右または左の前腕部１１Ｄあるいは胴体部１１Ａに対する角度を定義するデータである。スキャン角度範囲は、レーザ光をスキャンする角度の範囲を規定するデータである。スキャン角度範囲は、例えば水平方向に２４０度、上下方向に９０度などのスキャンする角度の範囲である。ライダ構造データ８４は、測距センサである測距ライダ１６Ｒ、１６Ｌ、１６Ｂの構造を表す測距センサ構造データである。

状態データ記憶部３３Ａは、ロボット３次元モデル６４Ａを変更し、カメラ周囲点群データ７１、ライダ点群生データ７２、ライダ点群データ７４およびライダ周囲点群データ７５を追加している。ロボット３次元モデル６４Ａは、ライダ姿勢７３も有する。ライダ姿勢７３は、測距ライダ１６Ｒ、１６Ｌ、１６Ｂの現場座標系における位置と放射方向を表すデータである。ライダ姿勢７３は、測距ライダ１６Ｒ、１６Ｌ、１６Ｂの３次元モデルである。

ライダ点群生データ７２は、測距ライダ１６Ｒ、１６Ｌ、１６Ｂが計測した点群のデータである。ライダ点群生データ７２は、測距ライダ１６Ｒ、１６Ｌ、１６Ｂの放射方向を基準とするデータである。ライダ点群生データ７２において、スキャンする範囲内において隣接する方向との間で計測した距離が大きく変化する方向を不連続点として認識する。ライダ点群データ７４は、ライダ点群生データ７２を現場座標系における値に変換したデータである。ライダ点群データ７４においても、不連続点を認識しておく。

ロボットモデル生成部３４Ａは、ライダ構造データ８４も参照して、カメラ姿勢６５およびライダ姿勢７３を含むロボット３次元モデル６４Ａを生成する。

環境モデル生成部３５Ａは、不連続点抽出部４７、ライダ座標変換部４８、ライダ点群整合部４９および点群選択生成部５０を追加し、カメラ点群整合部４６Ａを変更している。カメラ点群整合部４６Ａは、周囲点群データ６８ではなくカメラ周囲点群データ７１を生成する。カメラ点群整合部４６Ａは、カメラ点群整合部４６と同様にカメラ点群データ６７を処理する。カメラ周囲点群データ７１は、カメラ周囲環境モデルである。カメラ周囲環境モデルは、右カメラ２Ｒおよび左カメラ２Ｌが撮影した画像から生成される周囲環境モデルである。環境モデル生成部３５Ａは、カメラ周囲環境モデルを生成する画像処理モデル生成部である。

不連続点抽出部４７は、ライダ点群生データ７２を取り込み、ライダ点群生データ７２を処理して不連続点を抽出する。ライダ座標変換部４８は、ライダ姿勢７３を参照してライダ点群生データ７２を現場座標系での位置データであるライダ点群データ７４に変換する。ライダ点群整合部４９は、ライダ周囲点群データ７５を生成する。ライダ周囲点群データ７５は、周囲環境として存在する各事物への方向と距離を表現するデータである。ライダ点群整合部４９は、異なるライダ姿勢７３でのライダ点群データ７４を不連続点の位置が一致または位置の差が小さくなるようにつなげてライダ周囲点群データ７５を生成する。異なるライダ姿勢７３とは、少なくとも１個の測距ライダ１６の位置および姿勢の少なくとも一方が異なることである。同じライダ姿勢７３でのライダ点群データ７４の平均をとってライダ点群データ７４とし、あるライダ姿勢７３でのライダ点群データ７４を、別のライダ姿勢７３でのライダ点群データ７４と連結して、ライダ周囲点群データ７５を生成してもよい。ライダ周囲点群データ７５は、周囲環境までの距離と方向を含む測距データである。測距ライダ１６Ｒ、１６Ｌ、１６Ｂは、測距データを計測する測距センサである。ライダ周囲点群データ７５は、センサ周囲環境モデルである。センサ周囲環境モデルは、測距データから生成された周囲環境モデルである。環境モデル生成部３５Ａは、測距センサの位置と測距方向に基づき測距データを処理して、センサ周囲環境モデルを生成する測距データ処理モデル生成部である。

ライダ周囲点群データ７５を生成した後に新たなライダ点群データ７４が生成される場合は、ライダ点群整合部４９は、ライダ点群データ７４によりライダ周囲点群データ７５を修正する。新たなライダ点群データ７４で事物の位置が変化している場合は、その事物に関するライダ周囲点群データ７５での点群データは、新たな位置に移動させる。

制御装置３Ａにおける周囲点群データ６８は、カメラ周囲点群データ７１またはライダ周囲点群データ７５の何れか一方を選択した点群データである。点群選択生成部５０は、カメラ周囲点群データ７１またはライダ周囲点群データ７５の何れか一方を選択して、周囲点群データ６８とする。カメラ周囲点群データ７１が生成できる場合は、点群選択生成部５０はカメラ周囲点群データ７１を周囲点群データ６８とする。

図１４を参照して、動作を説明する。図１４は、実施の形態２に係る遠隔制御マニピュレータシステムでロボットを遠隔制御する手順を説明するフローチャートである。図１４について、実施の形態１の場合の図８とは異なる点を説明する。

ステップＳ０１Ａで、ライダ構造データ８４も構造データ記憶部３２Ａに記憶させる。

ステップＳ０３Ａで、測距ライダ１６Ｒ、１６Ｌ、１６Ｂに対しても補正パラメータを求める。ロボット１Ａに校正用の姿勢をとらせて、ロボット１Ａに対して決められた位置に定規物体を配置する。右カメラ２Ｒおよび左カメラ２Ｌは、定規物体を撮影する。撮影した画像から定規物体までの距離と定規物体の長さを計測する。測距ライダ１６Ｒ、１６Ｌ、１６Ｂは、定規物体が含まれる空間をレーザ光でスキャンして、定規物体の方向と距離を計測する。何点かの距離で、右カメラ２Ｒおよび左カメラ２Ｌは定規物体を撮影して計測し、測距ライダ１６Ｒ、１６Ｌ、１６Ｂは、定規物体が含まれる空間をレーザ光でスキャンして、定規物体の方向と距離を計測する。計測した距離および長さが実際の距離および長さと一致するように、右カメラ２Ｒおよび左カメラ２Ｌの換算係数とオフセット値を求め、測距ライダ１６Ｒ、１６Ｌ、１６Ｂの換算係数とオフセット値を求める。

ステップＳ０４Ａで、物体２０とその周囲環境の３次元モデルであるカメラ周囲点群データ７１およびライダ周囲点群データ７５を初期生成する。Ｓ０４Ａでの動作は、図１５に示すフローチャートで説明する。

ステップＳ０６Ａで、右カメラ２Ｒおよび左カメラ２Ｌが物体２０とその周囲を撮影し、測距ライダ１６Ｒ、１６Ｌ、１６Ｂがレーザ光をスキャンし、測距しながらヒューマノイド部１ＨＡを動かして物体２０を片方のハンド部１１Ｅで把持させる。Ｓ０６Ａでの動作は、図１６と図１７に示すフローチャートで説明する。図１６と図１７は、実施の形態２に係る遠隔制御マニピュレータシステムでロボットが物体を把持する際に遠隔制御する手順を説明するフローチャートである。図１６がフローチャートの前半部で、図１７が後半部である。図１６は、最初にモデル画像５３を表示するまでの手順を示す。図１７は、３Ｄ画像５２およびモデル画像５３を見ながらオペレータ３０が腕部１１Ｊを操作して物体２０を把持するまでの手順を示す。

図１５は、実施の形態２に係る遠隔制御マニピュレータシステムで周囲点群データを初期生成する手順を説明するフローチャートである。図１５について、実施の形態１の場合の図９とは異なる点を説明する。ステップＳ１３Ａで、姿勢センサ９２が姿勢データ５７を計測し、ライダ姿勢７３およびカメラ姿勢６５を含むロボット３次元モデル６４Ａを生成する。

ステップＳ１４Ａで、右カメラ２Ｒおよび左カメラ２Ｌが物体２０を含む画像を撮影し、撮影した画像からカメラ周囲点群データ７１を生成または修正する。Ｓ１５Ａで、必要十分な範囲および精度でカメラ周囲点群データ７１が生成できたかチェックする。周囲点群データ６８ではなくカメラ周囲点群データ７１を処理対象とする点で、Ｓ１４ＡおよびＳ１５ＡはＳ１４およびＳ１５とは異なる。

必要十分な範囲および精度でカメラ周囲点群データ７１が生成できている（Ｓ１５ＡでＹＥＳ）場合は、ステップＳ１９～Ｓ２５でライダ周囲点群データ７５を生成する。Ｓ１９で、測距ライダ１６Ｒ、１６Ｌ、１６Ｂがレーダ光をスキャンしながら放射し、ライダ点群データ７４を生成し、ライダ周囲点群データ７５を生成または修正する。

ステップＳ２０で、必要十分な範囲および精度でライダ周囲点群データ７５が生成できたかチェックする。必要十分な範囲および精度でライダ周囲点群データ７５が生成できている（Ｓ２０でＹＥＳ）場合は、処理を終了する。

必要十分な範囲および精度でライダ周囲点群データ７５が生成できていない（Ｓ２０でＮＯ）場合は、ステップＳ２１で別の姿勢をとるかチェックする。別の姿勢をとる（Ｓ２１でＹＥＳ）場合は、ステップＳ２２でヒューマノイド部１ＨＡに別の姿勢をとらせる。

別の姿勢をとらない（Ｓ２１でＮＯ）場合は、ステップＳ２３でロボット１を別の位置に移動させる。ステップＳ２４で、位置センサ９１が車両姿勢５６を計測する。Ｓ２４またはＳ２２の実行後に、ステップＳ２５で、姿勢センサ９２が姿勢データ５７を計測し、ライダ姿勢７３およびカメラ姿勢６５を含むロボット３次元モデル６４Ａを生成する。そして、Ｓ１９へ戻る。

図１６と図１７について、実施の形態１の場合の図１０とは異なる点を説明する。図１６と図１７に示すフローチャートは、カメラ周囲点群データ７１またはライダ周囲点群データ７５の何れか一方を周囲点群データ６８とする場合である。図１６と図１７に示すフローチャートでは、カメラ周囲点群データ７１を優先して使用する。また、カメラ点群データ６７を作成できない場合は、ライダ点群データ７４だけから周囲点群データ６８を生成する。ライダ点群データ７４だけから周囲点群データ６８を生成する状態で、カメラ点群データ６７を作成できるかチェックしてもよい。カメラ点群データ６７を作成できる場合は、カメラ点群データ６７およびカメラ周囲点群データ７１を生成して、カメラ周囲点群データ７１を優先して使用して周囲点群データ６８を生成してもよい。

ステップＳ３２Ａで、ライダ姿勢７３およびカメラ姿勢６５を含むロボット３次元モデル６４Ａを生成する。

ステップＳ３８Ａで、カメラ点群データ６７によりカメラ周囲点群データ７１を修正する。そして、ステップＳ４６で、表示状態をカメラ利用可と設定し、点群選択生成部５０がカメラ周囲点群データ７１を選択して周囲点群データ６８とする。ここで、表示状態は、点群選択生成部５０がカメラ周囲点群データ７１を選択できるかどうかを制御するフラグである。そして、Ｓ３９で、モデル統合部３６が周囲点群データ６８とロボット３次元モデル６４Ａを統合して統合３次元モデル６９Ａを生成する。

右カメラ２Ｒおよび左カメラ２Ｌの両方の視野内に物体２０が存在しない（Ｓ３５でＮＯ）場合、あるいはカメラ点群データ６７を生成できていない（Ｓ３７でＮＯ）場合は、ステップＳ４７～Ｓ５４でライダ周囲点群データ７５を生成する。Ｓ４７で、測距ライダ１６Ｒ、１６Ｌ、１６Ｂがスキャンしながらレーザ光を放射して周囲の事物との距離を計測し、ライダ点群データ７４を生成する。なお、不連続点抽出部４７が不連続点を抽出できない場合は、ライダ点群データ７４を生成できていないとする。ステップＳ４８で、ライダ点群データ７４が生成できているかチェックする。ライダ点群データ７４が生成できている（Ｓ４８でＹＥＳ）場合は、ステップＳ４９で、ライダ点群データ７４によりライダ周囲点群データ７５を修正する。ステップＳ５０で、表示状態をカメラ利用不可と設定し、ライダ周囲点群データ７５を選択して周囲点群データ６８とする。そして、Ｓ３９へ進む。

ライダ点群データ７４が生成できていない（Ｓ４８でＮＯ）場合は、周囲環境に存在する障害物などが原因でライダでの測距ができない場合に対応する。ライダ点群データ７４が生成できていない（Ｓ４８でＮＯ）場合は、ステップＳ５１で、測距ライダ１６Ｒ、１６Ｌの少なくとも一方が不連続点を有するライダ点群データ７４が生成できるように、物体２０を別視点から見る位置に右および左の腕部１１Ｊの少なくとも一方を移動させる。

ステップＳ５２で、制御周期Ｔｃが経過したかチェックする。Ｔｃが経過している（Ｓ５２でＹＥＳ）場合は、ステップＳ５３で、姿勢センサ９２が姿勢データ５７を計測する。ステップＳ５４で、車両姿勢５６および姿勢データ５７からカメラ姿勢６５およびライダ姿勢７３を含むロボット３次元モデル６４Ａを生成する。Ｓ５４の実行後またはＴｃが経過していない（Ｓ５２でＮＯ）場合は、Ｓ４７へ戻る。

オペレータ３０がヒューマノイド部１ＨＡを操作中の処理を示す図１７について説明する。Ｓ４３からＳ４５は、図１０と同様である。Ｓ４３で、オペレータ３０がヒューマノイド部１ＨＡを動かす。Ｓ４４で、ハンド部１１Ｅの感圧センサ９５の出力(触覚データ６０)が接触ありかチェックする。感圧センサ９５の出力が接触ありであることは、ハンド部１１Ｅが物体に接触していることを意味する。感圧センサ９５の出力が接触ありである（Ｓ４４でＹＥＳ）場合は、処理を終了する。感圧センサ９５の出力が接触ありではない（Ｓ４４でＮＯ）場合は、Ｓ４５で、決められた制御周期Ｔｃが経過したかチェックする。

Ｔｃが経過している（Ｓ４５でＹＥＳ）場合は、ステップＳ５５で、姿勢センサ９２が姿勢データ５７を計測する。ステップＳ５６で、車両姿勢５６および姿勢データ５７からカメラ姿勢６５およびライダ姿勢７３を含むロボット３次元モデル６４Ａを生成する。Ｓ５６の実行後またはＴｃが経過していない（Ｓ４５でＮＯ）場合は、ステップＳ５７で、右カメラ２Ｒおよび左カメラ２Ｌが物体２０の画像を撮影する。

ステップＳ５８で、立体視表示装置４が３Ｄ画像５２を立体視表示する。Ｓ５８と並行してステップＳ５９で、表示状態がカメラ利用可かチェックする。カメラ利用可である（Ｓ５９でＹＥＳ）場合は、ステップＳ６０で右カメラ２Ｒおよび左カメラ２Ｌの両方の視野内に物体２０が存在するかチェックする。右カメラ２Ｒおよび左カメラ２Ｌの両方の視野内に物体２０が存在する（Ｓ６０でＹＥＳ）場合は、ステップＳ６１で、撮影した画像からカメラ点群データ６７を生成する。ステップＳ６２で、カメラ点群データ６７によりカメラ周囲点群データ７１を修正する。ステップＳ６３で、カメラ周囲点群データ７１を選択して周囲点群データ６８とする。ステップＳ６４で、周囲点群データ６８とロボット３次元モデル６４Ａを統合して統合３次元モデル６９を生成する。ステップＳ６５で、統合３次元モデル６９からモデル画像５３を生成する。ステップＳ６６で、表示装置５がモデル画像５３を表示する。ステップＳ６７で、オペレータ３０が立体視表示装置４に立体視表示される３Ｄ画像５２または表示装置５に表示されたモデル画像５３を見る。そして、Ｓ４３へ戻る。

表示状態がカメラ利用可ではない（Ｓ５９でＮＯ）場合、あるいは右カメラ２Ｒおよび左カメラ２Ｌの何れか少なくとも一方の視野内に物体２０が存在しない（Ｓ６０でＮＯ）場合は、ステップＳ６８で、測距ライダ１６Ｒ、１６Ｌ、１６Ｂは、スキャンしながらレーザ光を放射して測距し、ライダ点群データ７４を生成する。ステップＳ６９で、ライダ点群データ７４によりライダ周囲点群データ７５を修正する。ステップＳ７０で、表示状態をカメラ利用不可とし、点群選択生成部５０がライダ周囲点群データ７５を選択して周囲点群データ６８とする。そして、Ｓ６４へ進む。

遠隔制御マニピュレータシステム１００Ａでも、オペレータ３０は、立体視表示された３Ｄ画像５２およびモデル画像５３を自由に切り替えて見ることができる。モデル画像５３は、オペレータ３０が指定した視点からロボット１および物体２０を見る画像である。オペレータ３０は、ロボット１Ａを遠隔制御して、物体２０を押す、引く、把持する等の作業を、従来よりも正確に、かつ容易に実施できる。その他の点でも、遠隔制御マニピュレータシステム１００Ａは、遠隔制御マニピュレータシステム１００と同様に動作して、同様な効果が得られる。

ロボット１Ａが測距ライダ１６を備えるので、物体２０およびその周囲環境の３次元モデルである周囲環境モデルを、遠隔制御マニピュレータシステム１００Ａは、遠隔制御マニピュレータシステム１００よりも容易にかつ正確に生成できる。

周囲点群データは、ライダ周囲点群データ７５を優先して使用してもよい。周囲点群データは、カメラ周囲点群データ７１またはライダ周囲点群データ７５の両方から生成した点群データでもよい。点群選択生成部５０は、カメラ周囲点群データ７１またはライダ周囲点群データ７５の何れか一方を選択して、周囲点群データ６８とする。あるいは、点群選択生成部５０は、カメラ周囲点群データ７１またはライダ周囲点群データ７５の両方から周囲点群データ６８を生成する。点群選択生成部５０は、カメラ周囲点群データ７１およびライダ周囲点群データ７５の少なくとも一方を選択して周囲点群データ６８を生成する。

点群選択生成部は、カメラ周囲点群データ７１およびライダ周囲点群データ７５の平均をとるなどして周囲点群データ６８としてもよい。あるいは、点群選択生成部は、周囲点群データ６８の一部はカメラ周囲点群データ７１を選択し、別の一部はライダ周囲点群データ７５を選択してもよい。点群選択生成部にカメラ周囲点群データ７１およびライダ周囲点群データ７５の両方が入力される場合に、点群選択生成部が生成する周囲点群データ６８をカメラ周囲点群データ７１およびライダ周囲点群データ７５を融合した融合周囲環境モデルと呼ぶ。その場合の点群選択生成部は、融合周囲環境モデルである周囲点群データ６８を生成する周囲環境モデル融合部である。点群選択生成部は、カメラ周囲点群データ７１またはライダ周囲点群データ７５の両方から生成する部分を有しない周囲点群データ６８を生成してもよい。周囲点群データ６８は、カメラ周囲点群データ７１である部分を有しないものでもよい。周囲点群データ６８は、ライダ周囲点群データ７５である部分を有しないものでもよい。

カメラ周囲点群データ７１およびライダ周囲点群データ７５の両方とも周囲点群データとして選択して、統合３次元モデルを生成してもよい。統合３次元モデルに基づき生成されるモデル画像には、カメラ周囲点群データ７１およびライダ周囲点群データ７５の両方が表示される。両方を表示する場合には、カメラ周囲点群データ７１とライダ周囲点群データ７５とで画像の表示色を変え、どちらの画像も透過する画像とする。透過する画像とすることで、カメラ周囲点群データ７１のライダ周囲点群データ７５の両方の画像を見ることができ、カメラ周囲点群データ７１のライダ周囲点群データ７５の背後のロボット１Ａや周囲環境も見ることができる。

カメラ周囲点群データ７１またはライダ周囲点群データ７５のどちらを優先して表示するか、両方を表示するか、あるいはカメラ周囲点群データ７１およびライダ周囲点群データ７５を融合した融合周囲環境モデル表示するかを選択する指示を、オペレータ３０が入力できるようにしてもよい。物体２０を扱う処理を開始した後に、オペレータ３０が表示方法を変更できるようにしてもよい。

環境モデル生成部は、画像処理によりカメラ周囲点群データを生成せず、ライダ周囲点群データ７５だけを生成するものでもよい。その場合の環境モデル生成部は、周囲環境モデルであるライダ周囲点群データ７５とロボット３次元モデル６４Ａとを統合するモデル統合部を有する。

測距ライダではなく、電波や超音波を放射することで周囲の物体までの距離を計測する測距センサを使用してもよい。必要十分なスキャン角度範囲および角度分解能で方向ごとの事物までの距離を計測できるものであれば、測距センサはどのようなものを使用してもよい。
以上のことは、他の実施の形態にもあてはまる。

実施の形態３．
実施の形態３は、ロボットの３次元モデルを、目標姿勢に基づき生成するように実施の形態２を変更した場合である。目標姿勢は、オペレータ３０が意図している制御後のロボット１Ａの姿勢を表すデータである。図１８を参照して、実施の形態３に係る遠隔制御マニピュレータシステムの構成を説明する。図１８は、実施の形態３に係る遠隔制御マニピュレータシステムの機能構成を示すブロック図である。

遠隔制御マニピュレータシステム１００Ｂの機能構成を示す図１８について、実施の形態２の場合の図１３とは異なる点を説明する。遠隔制御マニピュレータシステム１００Ｂを構成する制御装置３Ｂは、状態データ記憶部３３Ｂ、ロボットモデル生成部３４Ｂ、モデル統合部３６Ｂおよびモデル画像生成部３８Ｂを変更している。

状態データ記憶部３３Ｂは、２個のロボット３次元モデル６４Ａ、６４Ｂを記憶する。状態データ記憶部３３Ｂは、統合３次元モデル６９Ｂとモデル画像５３Ｂを記憶する。ロボット３次元モデル６４Ａは、現在のロボット１Ａの姿勢を表す姿勢データ５７に基づき生成されるロボット１Ａの３次元モデルである。ロボット３次元モデル６４Ｂは、目標姿勢５４に基づき生成されるロボット１Ａの３次元モデルである。目標姿勢５４は、オペレータ３０が入力する動作指示を実行後のロボット１Ａの位置および姿勢を表すデータである。統合３次元モデル６９Ｂおよびモデル画像５３Ｂは、ロボット３次元モデル６４Ｂに基づき生成される。説明を分かりやすくするために、ロボット３次元モデル６４Ａを現在３次元モデル６４Ａと呼び、ロボット３次元モデル６４Ｂを目標３次元モデル６４Ｂと呼ぶ。目標３次元モデル６４Ｂは、オペレータ３０が入力した目標姿勢５４をロボット１Ａがとる３次元モデルである。目標３次元モデル６４Ｂは、ロボット１Ａが動作指示に従う動作を実行後のマニピュレータモデルである目標マニピュレータモデルである。モデル画像５３Ｂは、目標姿勢５４をとるロボット１Ａを指定された視点から見る画像である。

カメラ座標変換部４５は、現在３次元モデル６４Ａに含まれるカメラ姿勢６５に基づき、カメラ点群生データ６６をカメラ点群データ６７に変換する。ライダ座標変換部４８は、現在３次元モデル６４Ａに含まれるライダ姿勢７３に基づき、ライダ点群生データ７２をライダ点群データ７４に変換する。

統合３次元モデル６９Ｂは、目標３次元モデル６４Ｂと現在の周囲環境を表す周囲点群データ６８を統合した３次元モデルを表すデータである。状態データ記憶部３３Ｂは、現在３次元モデル６４Ａと周囲点群データ６８を統合した統合３次元モデルは記憶しない。モデル画像５３Ｂは、統合３次元モデル６９Ｂを視点データ７０で指定される視点から見た画像である。モデル画像５３Ｂは、表示装置５に表示される。

ロボットモデル生成部３４Ｂは、姿勢データ５７を参照して現在３次元モデル６４Ａを生成し、目標姿勢５４を参照して目標３次元モデル６４Ｂを生成する。ロボットモデル生成部３４Ｂは、目標マニピュレータモデルを生成する目標マニピュレータモデル生成部である。

モデル統合部３６Ｂは、目標３次元モデル６４Ｂと周囲点群データ６８とを統合して統合３次元モデル６９Ｂを生成する。

モデル画像生成部３８Ｂは、統合３次元モデル６９Ｂを使用してモデル画像５３Ｂを生成する。

図１９を参照して、動作を説明する。図１９は、実施の形態に係る遠隔制御マニピュレータシステムでロボットを遠隔制御する手順を説明するフローチャートである。図１９について、実施の形態２の場合の図１４とは異なる点を説明する。

周囲点群データ６８を初期生成する際には、モデル画像５３Ｂをオペレータ３０に提示しないので、Ｓ０４Ａは変更していない。なお、周囲点群データ６８を初期生成する際にモデル画像５３Ｂを提示する場合には、Ｓ０４Ａを変更する。変更したＳ０４Ａを実行するフローチャートでは、図１５に示す周囲点群データ６８を初期生成する手順を説明するフローチャートを、以下のように修正する。Ｓ１３Ａ、Ｓ２５で、ロボットモデル生成部３４Ｂが、ライダ姿勢７３とカメラ姿勢６５を含む現在３次元モデル６４Ａと目標３次元モデル６４Ｂを生成する。Ｓ１３ＡまたはＳ２５次の手順として、モデル画像５３Ｂを表示装置５に表示する手順が追加される。

ステップＳ０６Ｂは、目標姿勢５４に基づくモデル画像５３Ｂをオペレータ３０に提示する点が、Ｓ０６Ａとは異なる。Ｓ０６Ｂで、右カメラ２Ｒおよび左カメラ２Ｌが物体２０とその周囲を撮影し、測距ライダ１６Ｒ、１６Ｌ、１６Ｂがレーザ光をスキャンし測距しながら、ヒューマノイド部１ＨＡを動かして物体２０を片方のハンド部１１Ｅで把持させる。Ｓ０６Ｂでの動作は、図２０と図２１に示すフローチャートで説明する。図２０と図２１は、実施の形態２に係る遠隔制御マニピュレータシステムでロボットが物体を把持する際に遠隔制御する手順を説明するフローチャートである。

ステップＳ３２Ｂ、Ｓ５４Ｂ、Ｓ５６Ｂで、ロボットモデル生成部３４Ｂが、ライダ姿勢７３とカメラ姿勢６５を含む現在３次元モデル６４Ａと目標３次元モデル６４Ｂを生成する。

ステップＳ３９Ｂ、Ｓ６４Ｂで、モデル統合部３６Ｂが、目標３次元モデル６４Ｂと周囲点群データ６８とを統合して統合３次元モデル６９Ｂを生成する。

ステップＳ４０Ｂ、Ｓ６５Ｂで、モデル画像生成部３８Ｂが、統合３次元モデル６９Ｂを使用してモデル画像５３Ｂを生成する。

なお、Ｓ３６などのカメラ点群生データ６６をカメラ点群データ６７に変換する処理は、実施の形態２の場合と同様に現在３次元モデル６４Ａに含まれるカメラ姿勢６５を使用する。Ｓ４７などのライダ点群生データ７２をライダ点群データ７４に変換する処理も、現在３次元モデル６４Ａに含まれるライダ姿勢７３を使用する。

遠隔制御マニピュレータシステム１００Ｂでも、オペレータ３０は、立体視表示された３Ｄ画像５２およびモデル画像５３Ｂを自由に切り替えて見ることができる。モデル画像５３Ｂは、オペレータ３０が指定した視点からロボット１および物体２０を見る画像である。オペレータ３０は、ロボット１Ａを遠隔制御して、物体２０を押す、引く、把持する等の作業を、従来よりも正確に、かつ容易に実施できる。その他の点でも、遠隔制御マニピュレータシステム１００Ｂは、遠隔制御マニピュレータシステム１００と同様に動作して、同様な効果が得られる。

オペレータ３０が見て確認するモデル画像５３Ｂは、目標姿勢５４に基づく目標３次元モデル６４Ｂに基づいて生成されている。オペレータ３０は、操作後のロボット１Ａの姿勢を実際にロボット１Ａが動くよりも前に確認できる。遠隔制御マニピュレータシステム１００Ｂでは、過剰な動きをロボット１Ａにさせることになる目標姿勢５４をオペレータ３０が入力することを防止できる。

遠隔制御マニピュレータシステム１００、１００Ａでは、通信や信号処理等による遅れ時間の影響により、オペレータ３０がロボット１Ａを動かしすぎる場合がある。具体的には、オペレータ３０が既に適切な目標姿勢５４を入力しているにも関わらず、モデル画像５３ではロボット１Ａが目標姿勢５４に至る途中の姿勢を表示する場合がある。そのようなモデル画像５３を見たオペレータ３０は、目標姿勢５４をさらに変化させる入力をする場合がある。その結果、ロボット１Ａが動きすぎてしまい、柔らかい物体２０を把持する場合などに、ロボット１Ａが把持することで物体２０が変形あるいは破損することが考えられる。物体２０が硬い場合でも、ロボット１Ａを操作する上で、ハンド部１１Ｅが想定するよりも早く物体２０と接触することになり、滑らかな操作ができにくくなると考えられる。

モデル画像として、ロボット１Ａの現在の姿勢を表す現在３次元モデル６４Ａと、制御の目標となる姿勢である目標姿勢を表す目標３次元モデル６４Ｂの両方を表示色や濃淡を変えるなどして同時に表示するモデル画像を表示してもよい。現在３次元モデル６４Ａおよび目標３次元モデル６４Ｂの両方を同時にモデル画像に表示する場合には、モデル統合部は現在３次元モデル６４Ａおよび目標３次元モデル６４Ｂの両方を選択して統合３次元モデルを生成する。ロボット１Ａの現在姿勢が示されるモデル画像５３、ロボット１Ａの目標姿勢が示されるモデル画像５３Ｂ、ロボット１Ａの現在姿勢および目標姿勢を同時に表示するモデル画像の何れかを、オペレータ３０が選択して表示できるようにしてもよい。この場合には、モデル統合部は現在３次元モデル６４Ａおよび目標３次元モデル６４Ｂの何れかまたは両方を選択して統合３次元モデルを生成する。モデル統合部は、現在３次元モデル６４Ａと周囲点群データ６８を統合した統合３次元モデル、目標３次元モデル６４Ｂと周囲点群データ６８を統合した統合３次元モデル、現在３次元モデル６４Ａおよび目標３次元モデル６４Ｂと周囲点群データ６８を統合した統合３次元モデルを生成してもよい。

周囲環境モデルも、画像から生成したカメラ周囲点群データ７１、測距ライダ１６の測距データに基づくライダ周囲点群データ７５の何れかを切り替えて表示したり、カメラ周囲点群データ７１およびライダ周囲点群データ７５を同時に区別できるように表示したりを、オペレータ３０が選択できるようにしてもよい。
以上のことは、他の実施の形態にもあてはまる。

実施の形態４．
実施の形態４は、既存の遠隔制御マニピュレータシステムに遠隔制御支援システムを付加して実施の形態３と同等な遠隔制御マニピュレータシステムを構成する場合である。図２２に、実施の形態４に係る遠隔制御マニピュレータシステムおよび遠隔制御支援システムの機能構成を示すブロック図である。図２２について、実施の形態３の場合の図１８とは異なる点を説明する。

既存の遠隔制御マニピュレータシステムは、例えばヘッドマウントディスプレイ（Head Mount Display、ＨＭＤ）をオペレータ３０が装着するシステムである。遠隔制御マニピュレータシステム１００Ｃは、ＨＭＤを立体視表示装置４に置き換え、支援装置２５、測距ライダ１６、表示装置５などを既存のシステムに追加して構成している。

ロボット１Ａ、制御装置３Ｃ、目標姿勢入力装置６は、既存のシステムでのものそのまま、あるいは必要な変更を施したものである。ロボット１Ａは、既存のロボット１に測距ライダ１６Ｒ、１６Ｌ、１６Ｂを搭載したものである。制御装置３Ｃは、既存の制御装置に目標姿勢５４を支援装置２５に送信する機能を持たせる。

制御装置３Ｃは、ロボット１Ａを遠隔制御する機能を有する。支援装置２５は、モデル画像５３Ｂを生成して表示装置５に表示する機能を有する。モデル画像５３Ｂは、目標姿勢をとるロボット３次元モデル６４Ｂと周囲点群データ６８を統合した統合３次元モデル６９Ｂを任意の視点から見る画像である。支援装置２５は、立体視表示装置４が３Ｄ画像５２を立体視表示する機能を有する。制御装置３Ｃが、ＨＭＤではなく立体視表示装置４に３Ｄ画像５２を出力するようにしてもよい。制御装置および支援装置を介さないで、立体視表示装置４に右目画像５１Ｒおよび左目画像５１Ｌが入力され、立体視表示装置４が入力される右目画像５１Ｒおよび左目画像５１Ｌを立体視表示してもよい。

支援装置２５、測距ライダ１６、立体視表示装置４、表示装置５は、遠隔制御支援システム１５０を構成する。図２２では、遠隔制御支援システム１５０の範囲を破線で示す。

支援装置２５は、構造データ記憶部２６、状態データ記憶部２７、通信部２８、ロボットモデル生成部３４Ｂ、環境モデル生成部３５Ａ、モデル統合部３６Ｂ、視点データ生成部３７、モデル画像生成部３８Ｂ、制御参考情報生成部３９を有する。

構造データ記憶部２６は、ロボット構造データ８１、カメラ構造データ８２、センサ構造データ８３、ライダ構造データ８４を有する。構造データ記憶部２６は、実施の形態３での構造データ記憶部３２Ａと同様である。

状態データ記憶部２７は、右目画像５１Ｒおよび左目画像５１Ｌで構成される３Ｄ画像５２、モデル画像５３Ｂ、目標姿勢５４、車両姿勢５６、姿勢データ５７、触覚データ６０、撮影条件データ６３、ロボット３次元モデル６４Ａ、６４Ｂを記憶する。目標姿勢５４、車両姿勢５６、姿勢データ５７および撮影条件データ６３は、ロボット３次元モデル６４Ａ、６４Ｂを生成するために使用される。触覚データ６０は、ハンド部１１Ｅが物体２０に接触しているかどうか判断するために使用される。ロボット３次元モデル６４Ａ、６４Ｂは、カメラ姿勢６５およびライダ姿勢７３を含む。状態データ記憶部２７は、さらにカメラ点群生データ６６、カメラ点群データ６７、周囲点群データ６８、統合３次元モデル６９Ｂ、視点データ７０、カメラ周囲点群データ７１、ライダ点群生データ７２、ライダ点群データ７４、ライダ周囲点群データ７５を記憶する。

通信部２８は、ロボット１Ａおよび制御装置３Ｃと通信する。通信部２８は、ロボット１Ａの通信部９から送信される車両姿勢５６、姿勢データ５７、触覚データ６０、撮影条件データ６３、右目画像５１Ｒ、左目画像５１Ｌおよびライダ点群生データ７２を受信し、制御装置３Ｃの通信部３１から送信される目標姿勢５４を受信する。通信部３１が、受信した車両姿勢５６、姿勢データ５７および触覚データ６０を、通信部２８に転送してもよい。

制御装置３Ｃは、ロボット１Ａを遠隔制御するためのデータおよび処理部を有する。制御装置３Ｃは、通信部３１、構造データ記憶部３２Ｃ、状態データ記憶部３３Ｃ、目標姿勢生成部４０、制御姿勢生成部４１、異常検出部４２を有する。状態データ記憶部３２Ｃは、ロボット構造データ８１、センサ構造データ８３を記憶する。状態データ記憶部３３Ｃは、目標姿勢５４、制御姿勢５５、車両姿勢５６、姿勢データ５７、モータ温度５８、物体距離５９、触覚データ６０、作用力データ６１、加速度データ６２を記憶する。

遠隔制御支援システム１５０を含む遠隔制御マニピュレータシステム１００Ｃは、遠隔制御マニピュレータシステム１００Ｂと同様に動作し同様な効果が得られる。

遠隔制御支援システム１５０によれば、オペレータ３０は、自分の頭を移動させることで、立体視表示された３Ｄ画像５２およびモデル画像５３Ｂを自由に切り替えて見ることができる。モデル画像５３Ｂは、オペレータ３０が指定した視点からロボット１および物体２０を見る画像である。遠隔制御支援システム１５０により提供される遠隔制御を支援する画像および情報を見ることで、オペレータ３０は、ロボット１を遠隔制御して、対象物を押す、引く、把持する等の作業を、従来よりも正確に、かつ容易に実施できる。

実施の形態１または実施の形態２と同様な遠隔制御マニピュレータシステム、あるいは他の遠隔制御マニピュレータシステムを構成する場合にも、本開示に係る遠隔制御支援システムは適用できる。制御装置と支援装置が別計算機として実装されない遠隔制御マニピュレータシステムにおいても、遠隔制御マニピュレータシステムは遠隔制御支援システムである部分を含む。遠隔制御支援システムである部分は、立体視表示装置と、ロボットおよび周囲環境の３次元モデルをオペレータ３０が指定した視点から見る画像であるモデル画像５３Ｂを生成する機能モジュール、モデル画像５３Ｂを表示する表示装置を含む部分である。

既存の立体視ができない遠隔制御マニピュレータシステムに遠隔制御支援システムを付加する場合は、右カメラおよび左カメラは遠隔制御支援システムに含まれる。

測距ライダ１６Ｒ、１６Ｌ、１６Ｂが計測するライダ点群生データ７２を、専用の通信装置により支援装置２５に送信してもよい。その場合には、制御部７Ａが有する記憶部１５Ａはライダ点群生データ７２を記憶しない。ロボット１Ａは、ハードウエアとしての測距ライダ１６Ｒ、１６Ｌ、１６Ｂが搭載されている点だけがロボット１とは異なる。ロボット１Ａは、測距ライダ１６Ｒ、１６Ｌ、１６Ｂが搭載されていないロボット１と同様に遠隔制御される。

各実施の形態の自由な組み合わせ、あるいは各実施の形態の変形や一部の構成要素を省略すること、あるいは一部の構成要素の省略や変形をした各実施の形態の自由な組み合わせが可能である。

１００、１００Ａ、１００Ｂ、１００Ｃ 遠隔制御マニピュレータシステム
１５０ 遠隔制御支援システム

２０ 物体
３０ オペレータ（操作者）

１、１Ａ クローラ移動ロボット（ロボット、マニピュレータ）
１Ｗ 車両部
１Ｈ、１ＨＡ ヒューマノイド部
２Ｒ 右カメラ
２Ｌ 左カメラ
３、３Ａ、３Ｂ 制御装置
４ 立体視表示装置
５ 表示装置（モデル画像表示装置）
６ 目標姿勢入力装置（動作指示入力部）
６Ｈ 上体目標姿勢入力部
６Ｗ 車両姿勢入力部
７ 制御部
８ 通信回線
９ 通信部
１０ ＬＡＮ

１１ 骨格部
１１Ａ 胴体部
１１Ｂ 腕基部
１１Ｃ 上腕部
１１Ｄ 前腕部
１１Ｅ ハンド部
１１Ｆ 頭部
１１Ｇ レッグ部
１１Ｈ 胴体接続部
１１Ｊ 腕部

１２ 関節部
１２Ａ 肩関節部
１２Ｂ 肘関節部
１２Ｃ 手首関節部
１２Ｄ 首関節部
１２Ｅ レッグ元関節部
１２Ｆ レッグ先関節部
１２Ｇ 胴体関節部

１３ アクチュエータ
１４ モータ
１５、１５Ａ 記憶部
１６Ｒ、１６Ｌ、１６Ｂ 測距ライダ（測距センサ）

２１ ＣＰＵ
２２ メモリ部
２３ ＣＰＵ
２４ メモリ部

２５ 支援装置
２６ 構造データ記憶部
２７ 状態データ記憶部
２８ 通信部

３１ 通信部
３２、３２Ａ、３２Ｃ 構造データ記憶部
３３、３３Ａ、３３Ｂ、３３Ｃ 状態データ記憶部
３４、３４Ａ ロボットモデル生成部（マニピュレータモデル生成部）
３４Ｂ ロボットモデル生成部（マニピュレータモデル生成部、目標マニピュレータモデル生成部）
３５ 環境モデル生成部（画像処理モデル生成部）
３５Ａ 環境モデル生成部（画像処理モデル生成部、測距データ処理モデル生成部）
３６、３６Ｂ モデル統合部
３７ 視点データ生成部
３８、３８Ｂ モデル画像生成部
３９ 制御参考情報生成部
４０ 目標姿勢生成部
４１ 制御姿勢生成部（制御信号生成部）
４２ 異常検出部
４３ 画像処理部
４４ カメラ点群生データ生成部
４５ カメラ座標変換部
４６、４６Ａ カメラ点群整合部
４７ 不連続点抽出部
４８ ライダ座標変換部
４９ ライダ点群整合部
５０ 点群選択生成部（周囲環境モデル融合部）

５１Ｒ 右目画像
５１Ｌ 左目画像
５２ ３Ｄ画像（立体視画像）
５３、５３Ｂ モデル画像
５４ 目標姿勢（動作指示）
５５ 制御姿勢（制御信号）
５６ 車両姿勢（位置姿勢情報）
５７ 姿勢データ（位置姿勢情報）
５８ モータ温度
５９ 物体距離
６０ 触覚データ
６１ 作用力データ
６２ 加速度データ
６３ 撮影条件データ
６４、６４Ａ ロボット３次元モデル（現在３次元モデル、マニピュレータモデル）
６４Ｂ ロボット３次元モデル（目標３次元モデル、目標マニピュレータモデル）
６５ カメラ姿勢
６６ カメラ点群生データ
６７ カメラ点群データ
６８ 周囲点群データ（周囲環境モデル、融合周囲環境モデル）
６９、６９Ａ、６９Ｂ 統合３次元モデル
７０ 視点データ
７１ カメラ周囲点群データ
７２ ライダ点群生データ
７３ ライダ姿勢
７４ ライダ点群データ
７５ ライダ周囲点群データ

８１ ロボット構造データ（マニピュレータ構造データ）
８２ カメラ構造データ
８３ センサ構造データ
８４ ライダ構造データ（測距センサ構造データ）

９１ 位置センサ（位置姿勢センサ）
９２ 姿勢センサ（位置姿勢センサ）
９３ 温度センサ
９４ 距離センサ
９５ 感圧センサ
９６ 力覚センサ
９７ 加速度センサ。

Claims (27)

1. 操作者により遠隔制御されて物体を扱うマニピュレータと、
   前記マニピュレータが動くまたは静止する動作を指示する動作指示を前記操作者が入力する動作指示入力部と、
   前記動作指示から前記マニピュレータを制御する制御信号を生成する制御信号生成部と、
   決められた位置関係を有して前記マニピュレータに搭載された右カメラおよび左カメラと、
   前記右カメラが撮影する右目画像および前記左カメラが撮影する左目画像を立体的に見えるように表示する立体視表示装置と、
   前記マニピュレータの位置および姿勢を表す位置姿勢情報を検出する位置姿勢センサと、
   前記マニピュレータの構造を表すマニピュレータ構造データ、および前記右カメラおよび前記左カメラの構造を表すカメラ構造データを記憶する構造データ記憶部と、
   前記マニピュレータ構造データ、前記カメラ構造データおよび前記位置姿勢情報を参照して、前記右カメラおよび前記左カメラを含む前記マニピュレータの３次元モデルであるマニピュレータモデルを生成するマニピュレータモデル生成部と、
   前記マニピュレータモデルに含まれる前記右カメラおよび前記左カメラの位置と撮影方向に基づき、前記右目画像および前記左目画像を画像処理して、前記物体および前記物体の周囲の事物である周囲環境の３次元モデルである周囲環境モデルを生成する画像処理モデル生成部と、
   前記周囲環境モデルと前記マニピュレータモデルを統合した３次元モデルである統合３次元モデルを生成するモデル統合部と、
   前記操作者が指定した視点から前記統合３次元モデルを見る画像であるモデル画像を生成するモデル画像生成部と、
   前記モデル画像を表示する、前記操作者が頭を移動させることで前記立体視表示装置と切り替えて見ることができるモデル画像表示装置とを備えた遠隔制御マニピュレータシステム。
2. 操作者により遠隔制御されて物体を扱うマニピュレータと、
   前記マニピュレータが動くまたは静止する動作を指示する動作指示を前記操作者が入力する動作指示入力部と、
   前記動作指示から前記マニピュレータを制御する制御信号を生成する制御信号生成部と、
   決められた位置関係を有して前記マニピュレータに搭載された右カメラおよび左カメラと、
   前記右カメラが撮影する右目画像および前記左カメラが撮影する左目画像を立体的に見えるように表示する立体視表示装置と、
   前記マニピュレータの位置および姿勢を表す位置姿勢情報を検出する位置姿勢センサと、
   前記マニピュレータに搭載されて前記物体および前記物体の周囲の事物である周囲環境までの距離と方向を含む測距データを計測する測距センサと、
   前記マニピュレータの構造を表すマニピュレータ構造データ、前記右カメラおよび前記左カメラの構造を表すカメラ構造データ、および前記測距センサの構造を表す測距センサ構造データを記憶する構造データ記憶部と、
   前記マニピュレータ構造データ、前記カメラ構造データ、前記測距センサ構造データ、および前記位置姿勢情報を参照して、前記右カメラ、前記左カメラおよび前記測距センサを含む前記マニピュレータの３次元モデルであるマニピュレータモデルを生成するマニピュレータモデル生成部と、
   前記マニピュレータモデルに含まれる前記右カメラおよび前記左カメラの位置と撮影方向に基づき、前記右目画像および前記左目画像を画像処理して、前記周囲環境の３次元モデルである周囲環境モデルを生成する画像処理モデル生成部と、
   前記マニピュレータモデルに含まれる前記測距センサの位置と測距方向に基づき、前記測距データを処理して前記周囲環境モデルを生成する測距データ処理モデル生成部と、
   前記画像処理モデル生成部が生成する前記周囲環境モデルであるカメラ周囲環境モデルと、前記測距データ処理モデル生成部が生成する前記周囲環境モデルであるセンサ周囲環境モデルとの少なくとも一方と前記マニピュレータモデルとを統合した３次元モデルである統合３次元モデルを生成するモデル統合部と、
   前記操作者が指定した視点から前記統合３次元モデルを見る画像であるモデル画像を生成するモデル画像生成部と、
   前記モデル画像を表示する、前記操作者が頭を移動させることで前記立体視表示装置と切り替えて見ることができるモデル画像表示装置とを備えた遠隔制御マニピュレータシステム。
3. 前記モデル統合部は、前記カメラ周囲環境モデルまたは前記センサ周囲環境モデルの何れか一方を選択して前記統合３次元モデルを生成する、請求項２に記載の遠隔制御マニピュレータシステム。
4. 前記モデル統合部は、前記カメラ周囲環境モデルおよび前記センサ周囲環境モデルの両方を選択して前記統合３次元モデルを生成する、請求項２に記載の遠隔制御マニピュレータシステム。
5. 前記モデル統合部は、前記操作者の指示により、前記カメラ周囲環境モデルおよび前記センサ周囲環境モデルの両方、あるいは前記カメラ周囲環境モデルおよび前記センサ周囲環境モデルの何れか一方を選択して前記統合３次元モデルを生成する、請求項２に記載の遠隔制御マニピュレータシステム。
6. 前記カメラ周囲環境モデルおよび前記センサ周囲環境モデルを融合した前記周囲環境モデルである融合周囲環境モデルを生成する周囲環境モデル融合部を備え、
   前記モデル統合部は、前記融合周囲環境モデルと前記マニピュレータモデルとを統合して前記統合３次元モデルを生成する、請求項２に記載の遠隔制御マニピュレータシステム。
7. 前記モデル統合部は、前記操作者の指示により、前記カメラ周囲環境モデルおよび前記センサ周囲環境モデルの両方、あるいは前記カメラ周囲環境モデル、前記センサ周囲環境モデルおよび前記融合周囲環境モデルの何れかを選択して前記統合３次元モデルを生成する、請求項６に記載の遠隔制御マニピュレータシステム。
8. 操作者により遠隔制御されて物体を扱うマニピュレータと、
   前記マニピュレータが動くまたは静止する動作を指示する動作指示を前記操作者が入力する動作指示入力部と、
   前記動作指示から前記マニピュレータを制御する制御信号を生成する制御信号生成部と、
   決められた位置関係を有して前記マニピュレータに搭載された右カメラおよび左カメラと、
   前記右カメラが撮影する右目画像および前記左カメラが撮影する左目画像を立体的に見えるように表示する立体視表示装置と、
   前記マニピュレータに搭載されて前記物体および前記物体の周囲の事物である周囲環境までの距離と方向を含む測距データを計測する測距センサと、
   前記マニピュレータの位置および姿勢を表す位置姿勢情報を検出する位置姿勢センサと、
   前記マニピュレータの構造を表すマニピュレータ構造データ、前記測距センサの構造を表す測距センサ構造データを記憶する構造データ記憶部と、
   前記マニピュレータ構造データ、前記測距センサ構造データ、および前記位置姿勢情報を参照して、前記測距センサを含む前記マニピュレータの３次元モデルであるマニピュレータモデルを生成するマニピュレータモデル生成部と、
   前記マニピュレータモデルに含まれる前記測距センサの位置と測距方向を参照して、前記測距データを処理して、前記周囲環境の３次元モデルである周囲環境モデルを生成する測距データ処理モデル生成部と、
   前記周囲環境モデルと前記マニピュレータモデルを統合した３次元モデルである統合３次元モデルを生成するモデル統合部と、
   前記操作者が指定した視点から前記統合３次元モデルを見る画像であるモデル画像を生成するモデル画像生成部と、
   前記モデル画像を表示する、前記操作者が頭を移動させることで前記立体視表示装置と切り替えて見ることができるモデル画像表示装置とを備えた遠隔制御マニピュレータシステム。
9. 複数の前記測距センサを備え、
   前記測距データ処理モデル生成部は、複数の前記測距センサが計測する複数の前記測距データを処理して前記周囲環境モデルを生成する、請求項２から請求項８の何れか１項に記載の遠隔制御マニピュレータシステム。
10. 前記測距データ処理モデル生成部は、前記測距センサの位置および姿勢の少なくとも一方が異なる際に計測された複数の前記測距データを、その前記測距データが計測された際の前記位置姿勢情報で生成された前記マニピュレータモデルに含まれる前記測距センサの位置と測距方向に基づき処理して前記周囲環境モデルを生成する、請求項２から請求項９の何れか１項に記載の遠隔制御マニピュレータシステム。
11. 前記画像処理モデル生成部は、前記右カメラおよび前記左カメラの少なくとも一方の位置および姿勢の少なくとも一方が異なる際に撮影された複数の前記右目画像および前記左目画像を、その前記右目画像および前記左目画像が撮影された際の前記位置姿勢情報で生成された前記マニピュレータモデルに含まれる前記右カメラおよび前記左カメラの位置と撮影方向に基づき画像処理して前記周囲環境モデルを生成する、請求項１から請求項７の何れか１項に記載の遠隔制御マニピュレータシステム。
12. 前記モデル画像生成部は、複数の異なる視点から前記統合３次元モデルを見る複数の前記モデル画像を生成し、
    前記モデル画像表示装置は、複数の前記モデル画像を同時に表示する、請求項１から請求項７、請求項１１の何れか１項に記載の遠隔制御マニピュレータシステム。
13. 前記動作指示に従う動作を実行後の前記マニピュレータの位置および姿勢を表す目標姿勢を参照して、前記動作指示に従う動作を実行後の前記マニピュレータモデルである目標マニピュレータモデルを生成する目標マニピュレータモデル生成部を備え、
    前記モデル統合部は、前記操作者の指示により、前記マニピュレータモデルおよび前記目標マニピュレータモデルの何れかまたは両方を選択して前記統合３次元モデルを生成する、請求項１から請求項１２の何れか１項に記載の遠隔制御マニピュレータシステム。
14. 操作者により遠隔制御されて物体を扱うマニピュレータに搭載されて決められた位置関係を有する右カメラおよび左カメラが撮影する右目画像および左目画像を立体的に見えるように表示する立体視表示装置と、
    前記マニピュレータの構造を表すマニピュレータ構造データ、および前記右カメラおよび前記左カメラの構造を表すカメラ構造データを記憶する構造データ記憶部と、
    前記マニピュレータ構造データ、前記カメラ構造データおよび前記マニピュレータの位置および姿勢を表す位置姿勢情報を参照して、前記右カメラおよび前記左カメラを含む前記マニピュレータの３次元モデルであるマニピュレータモデルを生成するマニピュレータモデル生成部と、
    前記マニピュレータモデルに含まれる前記右カメラおよび前記左カメラの位置と撮影方向に基づき、前記右目画像および前記左目画像を画像処理して、前記物体および前記物体の周囲の事物である周囲環境の３次元モデルである周囲環境モデルを生成する画像処理モデル生成部と、
    前記周囲環境モデルと前記マニピュレータモデルを統合した３次元モデルである統合３次元モデルを生成するモデル統合部と、
    前記操作者が指定した視点から前記統合３次元モデルを見る画像であるモデル画像を生成するモデル画像生成部と、
    前記モデル画像を表示する、前記操作者が頭を移動させることで前記立体視表示装置と切り替えて見ることができるモデル画像表示装置とを備えた遠隔制御支援システム。
15. 操作者により遠隔制御されて物体を扱うマニピュレータに搭載されて決められた位置関係を有する右カメラおよび左カメラが撮影する右目画像および左目画像を立体的に見えるように表示する立体視表示装置と、
    前記マニピュレータに搭載されて前記物体および前記物体の周囲の事物である周囲環境までの距離と方向を含む測距データを計測する測距センサと、
    前記マニピュレータの構造を表すマニピュレータ構造データ、前記右カメラおよび前記左カメラの構造を表すカメラ構造データ、および前記測距センサの構造を表す測距センサ構造データを記憶する構造データ記憶部と、
    前記マニピュレータ構造データ、前記カメラ構造データ、前記測距センサ構造データ、および前記マニピュレータの位置および姿勢を表す位置姿勢情報を参照して、前記右カメラ、前記左カメラおよび前記測距センサを含む前記マニピュレータの３次元モデルであるマニピュレータモデルを生成するマニピュレータモデル生成部と、
    前記マニピュレータモデルに含まれる前記右カメラおよび前記左カメラの位置と撮影方向に基づき、前記右目画像および前記左目画像を画像処理して、前記周囲環境の３次元モデルである周囲環境モデルを生成する画像処理モデル生成部と、
    前記マニピュレータモデルに含まれる前記測距センサの位置と測距方向に基づき、前記測距データを処理して前記周囲環境モデルを生成する測距データ処理モデル生成部と、
    前記画像処理モデル生成部が生成する前記周囲環境モデルであるカメラ周囲環境モデルと、前記測距データ処理モデル生成部が生成する前記周囲環境モデルであるセンサ周囲環境モデルとの少なくとも一方と前記マニピュレータモデルとを統合した３次元モデルである統合３次元モデルを生成するモデル統合部と、
    前記操作者が指定した視点から前記統合３次元モデルを見る画像であるモデル画像を生成するモデル画像生成部と、
    前記モデル画像を表示する、前記操作者が頭を移動させることで前記立体視表示装置と切り替えて見ることができるモデル画像表示装置とを備えた遠隔制御支援システム。
16. 前記モデル統合部は、前記カメラ周囲環境モデルまたは前記センサ周囲環境モデルの何れか一方を選択して前記統合３次元モデルを生成する、請求項１５に記載の遠隔制御支援システム。
17. 前記モデル統合部は、前記カメラ周囲環境モデルおよび前記センサ周囲環境モデルの両方を選択して前記統合３次元モデルを生成する、請求項１５に記載の遠隔制御支援システム。
18. 前記モデル統合部は、前記操作者の指示により、前記カメラ周囲環境モデルおよび前記センサ周囲環境モデルの両方、あるいは前記カメラ周囲環境モデルおよび前記センサ周囲環境モデルの何れかを選択して前記統合３次元モデルを生成する、請求項１５に記載の遠隔制御支援システム。
19. 前記カメラ周囲環境モデルおよび前記センサ周囲環境モデルを融合した前記周囲環境モデルである融合周囲環境モデルを生成する周囲環境モデル融合部を備え、
    前記モデル統合部は、前記融合周囲環境モデルと前記マニピュレータモデルとを統合して前記統合３次元モデルを生成する、請求項１５に記載の遠隔制御支援システム。
20. 前記モデル統合部は、前記操作者の指示により、前記カメラ周囲環境モデルおよび前記センサ周囲環境モデルの両方、あるいは前記カメラ周囲環境モデル、前記センサ周囲環境モデルおよび前記融合周囲環境モデルの何れかを選択して前記統合３次元モデルを生成する、請求項１９に記載の遠隔制御支援システム。
21. 操作者により遠隔制御されて物体を扱うマニピュレータに決められた位置関係を有して搭載された右カメラおよび左カメラが撮影する右目画像および左目画像を立体的に見えるように表示する立体視表示装置と、
    前記マニピュレータに搭載されて前記物体および前記物体の周囲の事物である周囲環境までの距離と方向を含む測距データを計測する測距センサと、
    前記マニピュレータの構造を表すマニピュレータ構造データ、前記測距センサの構造を表す測距センサ構造データを記憶する構造データ記憶部と、
    前記マニピュレータ構造データ、前記測距センサ構造データ、および前記マニピュレータの位置および姿勢を表す位置姿勢情報を参照して、前記測距センサを含む前記マニピュレータの３次元モデルであるマニピュレータモデルを生成するマニピュレータモデル生成部と、
    前記マニピュレータモデルに含まれる前記測距センサの位置と測距方向に基づき、前記測距データを処理して、前記周囲環境の３次元モデルである周囲環境モデルを生成する測距データ処理モデル生成部と、
    前記周囲環境モデルと前記マニピュレータモデルを統合した３次元モデルである統合３次元モデルを生成するモデル統合部と、
    前記操作者が指定した視点から前記統合３次元モデルを見る画像であるモデル画像を生成するモデル画像生成部と、
    前記モデル画像を表示する、前記操作者が頭を移動させることで前記立体視表示装置と切り替えて見ることができるモデル画像表示装置とを備えた遠隔制御支援システム。
22. 複数の前記測距センサを備え、
    前記測距データ処理モデル生成部は、複数の前記測距センサが計測する複数の前記測距データを処理して前記周囲環境モデルを生成する、請求項１５から請求項２１の何れか１項に記載の遠隔制御支援システム。
23. 前記測距データ処理モデル生成部は、前記測距センサの位置および姿勢の少なくとも一方が異なる際に計測された複数の前記測距データを、その前記測距データが計測された際の前記位置姿勢情報で生成された前記マニピュレータモデルに含まれる前記測距センサの位置と測距方向に基づき処理して前記周囲環境モデルを生成する、請求項１５から請求項２２の何れか１項に記載の遠隔制御支援システム。
24. 前記画像処理モデル生成部は、前記右カメラおよび前記左カメラの少なくとも一方の位置および姿勢少なくとも一方が異なる際に撮影された複数の前記右目画像および前記左目画像を、その前記右目画像および前記左目画像が撮影された際の前記位置姿勢情報で生成された前記マニピュレータモデルに含まれる前記右カメラおよび前記左カメラの位置と撮影方向に基づき画像処理して前記周囲環境モデルを生成する、請求項１４から請求項２０の何れか１項に記載の遠隔制御支援システム。
25. 前記モデル画像生成部は、複数の異なる視点から前記統合３次元モデルを見る複数の前記モデル画像を生成し、
    前記モデル画像表示装置は、複数の前記モデル画像を同時に表示する、請求項１４から請求項２０、請求項２４の何れか１項に記載の遠隔制御支援システム。
26. 前記マニピュレータが動くまたは静止する動作を指示する動作指示に従う動作を実行後の前記マニピュレータの位置および姿勢を表す目標姿勢を参照して、前記動作指示に従う動作を実行後の前記マニピュレータモデルである目標マニピュレータモデルを生成する目標マニピュレータモデル生成部を備え、
    前記モデル統合部は、前記操作者の指示により、前記マニピュレータモデルおよび前記目標マニピュレータモデルの何れかまたは両方を選択して前記統合３次元モデルを生成する、請求項１４から請求項２５の何れか１項に記載の遠隔制御支援システム。
27. 前記右カメラおよび前記左カメラを備えた、請求項１４から請求項２６の何れか１項に記載の遠隔制御支援システム。

Images