ホーム > 特許ランキング > 本田技研工業株式会社
知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024143149
(43)【公開日】2024-10-11
(54)【発明の名称】ロボット制御システム
(51)【国際特許分類】
B25J 11/00 20060101AFI20241003BHJP
【ＦＩ】
B25J11/00 Z
【審査請求】未請求
【請求項の数】5
【出願形態】ＯＬ
(21)【出願番号】P 2023055674
(22)【出願日】2023-03-30
(71)【出願人】
【識別番号】000005326
【氏名又は名称】本田技研工業株式会社
(74)【代理人】
【識別番号】100165179
【弁理士】
【氏名又は名称】田▲崎▼ 聡
(74)【代理人】
【識別番号】100126664
【弁理士】
【氏名又は名称】鈴木 慎吾
(74)【代理人】
【識別番号】100154852
【弁理士】
【氏名又は名称】酒井 太一
(74)【代理人】
【識別番号】100194087
【弁理士】
【氏名又は名称】渡辺 伸一
(72)【発明者】
【氏名】吉川 泰三
【テーマコード（参考）】
3C707
【Ｆターム（参考）】
3C707AS28
3C707BS10
3C707BS26
3C707HS27
3C707JS07
3C707JU14
3C707KS21
3C707KT01
3C707KT04
3C707KT15
3C707KV01
3C707KV08
3C707KW01
3C707KW03
3C707KX07
3C707LU06
3C707LV15
3C707LV19
(57)【要約】
【課題】操作者がカメラを観ながらロボットと手先を精度良く操作することができるロボット制御システムを提供することを目的とする。
【解決手段】ロボット制御システムは、地面にロボットが接していない空間でロボットを制御するロボット制御システムであって、ロボット本体と、ロボット本体に接続されるアームと、アームに接続されるエンドエフェクタと、エンドエフェクタの操作状況を撮影可能なカメラと、仮想絶対座標系を設定する設定部と、操作者によりカメラ座標系での指令を入力する入力部と、入力部に入力された指令を仮想絶対座標系で制御し、ロボット本体座標系に変換してロボット本体に指令する制御部と、を備える。
【選択図】図３
【解決手段】ロボット制御システムは、地面にロボットが接していない空間でロボットを制御するロボット制御システムであって、ロボット本体と、ロボット本体に接続されるアームと、アームに接続されるエンドエフェクタと、エンドエフェクタの操作状況を撮影可能なカメラと、仮想絶対座標系を設定する設定部と、操作者によりカメラ座標系での指令を入力する入力部と、入力部に入力された指令を仮想絶対座標系で制御し、ロボット本体座標系に変換してロボット本体に指令する制御部と、を備える。
【選択図】図３
【特許請求の範囲】
【請求項1】
地面にロボットが接していない空間で前記ロボットを制御するロボット制御システムであって、
ロボット本体と、
前記ロボット本体に接続されるアームと、
前記アームに接続されるエンドエフェクタと、
前記エンドエフェクタの操作状況を撮影可能なカメラと、
仮想絶対座標系を設定する設定部と、
操作者によりカメラ座標系での指令を入力する入力部と、
前記入力部に入力された指令を前記仮想絶対座標系で制御し、ロボット本体座標系に変換して前記ロボット本体に指令する制御部と、
を備えるロボット制御システム。
ロボット本体と、
前記ロボット本体に接続されるアームと、
前記アームに接続されるエンドエフェクタと、
前記エンドエフェクタの操作状況を撮影可能なカメラと、
仮想絶対座標系を設定する設定部と、
操作者によりカメラ座標系での指令を入力する入力部と、
前記入力部に入力された指令を前記仮想絶対座標系で制御し、ロボット本体座標系に変換して前記ロボット本体に指令する制御部と、
を備えるロボット制御システム。
【請求項2】
前記制御部は、
前記ロボットの位置と姿勢を、ＤＶＬ（Ｄｏｐｐｌｅｒ Ｖｅｌｏｃｉｔｙ Ｌｏｇ）センサ、Ｉｎｔｅｒｎａｌ ｐｒｅｓｓｕｒｅセンサ、慣性計測装置、および磁気センサの検出値を用いて推定する、
請求項１に記載のロボット制御システム。
前記ロボットの位置と姿勢を、ＤＶＬ（Ｄｏｐｐｌｅｒ Ｖｅｌｏｃｉｔｙ Ｌｏｇ）センサ、Ｉｎｔｅｒｎａｌ ｐｒｅｓｓｕｒｅセンサ、慣性計測装置、および磁気センサの検出値を用いて推定する、
請求項１に記載のロボット制御システム。
【請求項3】
前記制御部は、前記ロボット本体が移動モードから、作業モードに切り替わる際に前記仮想絶対座標系を設定する、
請求項１または請求項２に記載のロボット制御システム。
請求項１または請求項２に記載のロボット制御システム。
【請求項4】
前記仮想絶対座標系における原点である仮想座標原点は、前記ロボット本体の原点、前記カメラのパン軸の位置と異なる位置である、
請求項１または請求項２に記載のロボット制御システム。
請求項１または請求項２に記載のロボット制御システム。
【請求項5】
前記設定部は、前記仮想絶対座標系において、前記エンドエフェクタの可動範囲内に、仮想作業座標原点を設定する、
請求項１または請求項２に記載のロボット制御システム。
請求項１または請求項２に記載のロボット制御システム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、ロボット制御システムに関する。
【背景技術】
【0002】
水中を移動しながら作業を行う装置がある。このような装置は、スラスタ、撮影用のカメラ等を備えている（例えば特許文献１参照）。また、図２０のように、操縦は、例えば船上から行い、例えば操縦室にいる操作者と、水中で作業を行う装置と、装置を制御する制御装置とを接続するケーブルを操作するテザーマンとの間で情報交換を行いつつ行われる。操縦室の操作者は、画像表示装置に表示される画像を見ながら、カメラ画像基準でコントローラを操作して操作入力を決定し、テザーマンは甲板上から船基準でロボットの進行方向を操作者に教示している。図２０は、従来技術における船上での作業例を示す図である。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特許第５８０６５６８号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかしながら、船の向きは潮の流れで常に動いているため、例えば５分前は船頭に対して時計３時方向が、海中の作業空間では対象物に正面の方向であったのに、今は横方向になっているといった事象が起こり得る。また、水中で作業を行う装置のカメラから目標物を一度見失ってしまうと、操作者は、水中で作業を行う装置の進行方向の入力判断が難しくなる。このため、従来技術では、操作者は高度な操縦技術（空間把握能力）が要求されるという課題があった。
【0005】
本発明は、上記の問題点に鑑みてなされたものであって、操作者がカメラを観ながらロボットと手先を精度良く操作することができるロボット制御システムを提供することを目的とする。
【課題を解決するための手段】
【0006】
（１）上記目的を達成するため、本発明の一態様に係るロボット制御システムは、地面にロボットが接していない空間で前記ロボットを制御するロボット制御システムであって、ロボット本体と、前記ロボット本体に接続されるアームと、前記アームに接続されるエンドエフェクタと、前記エンドエフェクタの操作状況を撮影可能なカメラと、仮想絶対座標系を設定する設定部と、操作者によりカメラ座標系での指令を入力する入力部と、前記入力部に入力された指令を前記仮想絶対座標系で制御し、ロボット本体座標系に変換して前記ロボット本体に指令する制御部と、を備えるロボット制御システムである。
【0007】
（２）（１）のロボット制御システムにおいて、前記制御部は、前記ロボットの位置と姿勢を、ＤＶＬ（Ｄｏｐｐｌｅｒ Ｖｅｌｏｃｉｔｙ Ｌｏｇ）センサ、Ｉｎｔｅｒｎａｌ ｐｒｅｓｓｕｒｅセンサ、慣性計測装置、および磁気センサの検出値を用いて推定するようにしてもよい。
【0008】
（３）（１）または（２）のロボット制御システムにおいて、前記制御部は、前記ロボット本体が移動モードから、作業モードに切り替わる際に前記仮想絶対座標系を設定するようにしてもよい。
【0009】
（４）（１）から（３）のうちのいずれか１つのロボット制御システムにおいて、前記仮想絶対座標系における原点である仮想座標原点は、前記ロボット本体の原点、前記カメラのパン軸の位置と異なる位置であるようにしてもよい。
【0010】
（５）（１）から（３）のうちのいずれか１つのロボット制御システムにおいて、前記設定部は、前記仮想絶対座標系において、前記エンドエフェクタの可動範囲内に、仮想作業座標原点を設定するようにしてもよい。
【発明の効果】
【0011】
（１）～（５）によれば、操作者がカメラを観ながらロボットと手先を精度良く操作することができる。
【図面の簡単な説明】
【0012】
【図1】実施形態に係るロボット制御システムを上から見た外観例と概略構成例を示す図である。
【図2】実施形態に係るロボット制御システムを横から見た外観例を示す図である。
【図3】実施形態に係るロボット制御システムの構成例を示す図である。
【図4】絶対座標系、仮想絶対座標系、ロボットの座標系と、ロボット本体と手先の位置・姿勢位置を示す図である。
【図5】仮想座標系の初期状態で、ロボットの可動範囲とロボットの非可動範囲例を示す図である。
【図6】カメラ座標系での指令の座標変換を説明するための図である。
【図7】実施形態に係るロボット制御システムの処理手順のフローチャートである。
【図8】ロボット本体の前後の移動（ｘ軸方向）指令を決定する拘束を説明するための図である。
【図9】ロボット本体の左右の移動（ｙ軸方向）指令を決定する拘束を説明するための図である。
【図10】ロボット本体の上下の移動（ｚ軸方向）指令を決定する拘束を説明するための図である。
【図11】ロボット本体の回転移動（ｚ軸回転）指令を決定する拘束を説明するための図である。
【図12】回転中心がロボット本体の原点の場合の例を示す図である。
【図13】回転中心がカメラのパン軸の場合の例を示す図である。
【図14】回転中心が仮想的作業位置の場合の例を示す図である。
【図15】回転中心が左アームの手先位置の場合の例を示す図である。
【図16】ロボット本体の原点を回転中心に回転させた場合の撮影範囲がズレる例を示す図である。
【図17】カメラのパン軸を回転中心に回転させた場合の撮影範囲例を示す図である。
【図18】仮想的作業位置を設定しロボット本体の原点を中心に回転させた場合に撮影範囲がズレる例を示す図である。
【図19】仮想的作業位置を中心に回転させた場合の視野の例を示す図である。
【図20】従来技術における船上での作業例を示す図である。
【発明を実施するための形態】
【0013】
以下、本発明の実施の形態について図面を参照しながら説明する。なお、以下の説明に用いる図面では、各部材を認識可能な大きさとするため、各部材の縮尺を適宜変更している。
なお、実施形態を説明するための全図において、同一の機能を有するものは同一符号を用い、繰り返しの説明は省略する。
また、本願でいう「ＸＸに基づいて」とは、「少なくともＸＸに基づく」ことを意味し、ＸＸに加えて別の要素に基づく場合も含む。また、「ＸＸに基づいて」とは、ＸＸを直接に用いる場合に限定されず、ＸＸに対して演算や加工が行われたものに基づく場合も含む。「ＸＸ」は、任意の要素（例えば、任意の情報）である。
なお、実施形態を説明するための全図において、同一の機能を有するものは同一符号を用い、繰り返しの説明は省略する。
また、本願でいう「ＸＸに基づいて」とは、「少なくともＸＸに基づく」ことを意味し、ＸＸに加えて別の要素に基づく場合も含む。また、「ＸＸに基づいて」とは、ＸＸを直接に用いる場合に限定されず、ＸＸに対して演算や加工が行われたものに基づく場合も含む。「ＸＸ」は、任意の要素（例えば、任意の情報）である。
【0014】
（ロボット制御システムの外観例）
まず、ロボット制御システム１の外観例と概略構成例を説明する。なお、以下の各例では、ロボットの例として、水中で作業を行う水中ロボットまたは水中ドローンの例を説明するが、ロボットの作業空間はこれに限らない。ロボットの作業空間は、ロボット本体１０１が地面等に接していない空間、例えば空中や宇宙空間等であってもよい。また、実施形態において、ロボット本体を「ロボット」ともいう。
まず、ロボット制御システム１の外観例と概略構成例を説明する。なお、以下の各例では、ロボットの例として、水中で作業を行う水中ロボットまたは水中ドローンの例を説明するが、ロボットの作業空間はこれに限らない。ロボットの作業空間は、ロボット本体１０１が地面等に接していない空間、例えば空中や宇宙空間等であってもよい。また、実施形態において、ロボット本体を「ロボット」ともいう。
【0015】
図１は、本実施形態に係るロボット制御システムを上から見た外観例と概略構成例を示す図である。図２は、本実施形態に係るロボット制御システムを横から見た外観例を示す図である。
ロボット制御システム１は、例えば、ロボット本体１０１と、ロボット本体１０１に接続されるアーム１０２（１０２Ｌ、１０２Ｒ）と、カメラ１０７と、スラスタ１１１を備える。なお、アーム１０２は１つ以上備えていればよい。
アーム原点Ｐａ（Ｐａｌ、Ｐａｒ）は、アーム１０２がロボット本体１０１に接続される点付近に設定する。アーム原点は、アーム１０２の肩側の任意の箇所であってもよい。
符号Ｘｈ（Ｘｈｌ、Ｘｈｒ）は、アームの先端部の位置（アーム先端位置）を示している。
ロボット制御システム１は、例えば、ロボット本体１０１と、ロボット本体１０１に接続されるアーム１０２（１０２Ｌ、１０２Ｒ）と、カメラ１０７と、スラスタ１１１を備える。なお、アーム１０２は１つ以上備えていればよい。
アーム原点Ｐａ（Ｐａｌ、Ｐａｒ）は、アーム１０２がロボット本体１０１に接続される点付近に設定する。アーム原点は、アーム１０２の肩側の任意の箇所であってもよい。
符号Ｘｈ（Ｘｈｌ、Ｘｈｒ）は、アームの先端部の位置（アーム先端位置）を示している。
【0016】
図１、図２のように、アーム１０２は、いくつかのリンクが接続され、関節を備える。またアームの先端には、例えば、指部を備えるエンドエフェクタを備えている。
なお、図１、図２に示した外観は一例であり、これに限らない。また、アームの取り付け位置は、先端に限らず左右等であってもよい。
なお、図１、図２に示した外観は一例であり、これに限らない。また、アームの取り付け位置は、先端に限らず左右等であってもよい。
【0017】
【0018】
ロボット制御装置１００は、ロボット本体１０１と、アーム１０２と、エンドエフェクタ１０３と、駆動部１０４と、アームセンサ１０５と、設定部１０６と、カメラ１０７と、カメラ駆動部１０８と、カメラセンサ１０９と、照明１１０と、スラスタ１１１と、スラスタ駆動部１１２と、制御部１１３と、記憶部１１４と、通信部１１６と、センサ１１７を備える。
【0019】
操作部２００は、例えば、入力部２０１と、画像表示部２０２と、通信部２０３を備える。
【0020】
操作部２００は、例えば船上で操作者が使用する。なお、操作部２００とロボット制御装置１００とは、例えば互いに有線で接続されている。
入力部２０１は、操作者がロボット本体１０１やアーム１０２に対する操作指令を、入力する装置である。入力部２０１は、例えば、ハンドル、ジョイスティック、タッチパネルセンサ等である。なお、操作指令は、アーム１０２やエンドエフェクタ１０３に対する指示であってもよく、ロボット本体１０１に対する指示であってもよい。
画像表示部２０２は、カメラ１０７が撮影した画像や、ロボット本体１０１やアーム１０２の状態等をロボット制御装置１００から取得して表示する。
通信部２０３は、ロボット制御装置１００と情報の送受信を行う。
入力部２０１は、操作者がロボット本体１０１やアーム１０２に対する操作指令を、入力する装置である。入力部２０１は、例えば、ハンドル、ジョイスティック、タッチパネルセンサ等である。なお、操作指令は、アーム１０２やエンドエフェクタ１０３に対する指示であってもよく、ロボット本体１０１に対する指示であってもよい。
画像表示部２０２は、カメラ１０７が撮影した画像や、ロボット本体１０１やアーム１０２の状態等をロボット制御装置１００から取得して表示する。
通信部２０３は、ロボット制御装置１００と情報の送受信を行う。
【0021】
ロボット本体１０１は、図１において、例えばアーム１０２以外の部分である。ロボット本体１０１に、例えば、アーム１０２と、エンドエフェクタ１０３と、駆動部１０４と、設定部１０６と、カメラ１０７と、カメラ駆動部１０８と、カメラセンサ１０９と、照明１１０と、スラスタ１１１と、スラスタ駆動部１１２と、制御部１１３と、記憶部１１４と、通信部１１６と、センサ１１７が取り付けられている。なお、ロボット本体及び各機能部への電源供給は、ロボット制御装置１００が電源を備えていてもよく、例えば船上からケーブルで電源を供給するようにしてもよい。
【0022】
アーム１０２は、例えば図１のように関節を備える。アームの先端には、例えばエンドエフェクタ１０３が取り付けられている。関節それぞれには、例えばアクチュエータとエンコーダ（アームセンサ１０５）が取り付けられている。
【0023】
エンドエフェクタ１０３は、少なくとも２つの指部を備える。エンドエフェクタ１０３は、３つ以上の指部を備えていてもよく、グリッパー等であってもよい。また、エンドエフェクタ１０３の指先には、アームセンサ１０５（例えば６軸センサ、触覚センサ等）が取り付けられていてもよい。
【0024】
駆動部１０４は、制御指令に基づいてアーム１０２とエンドエフェクタ１０３を駆動する。駆動部１０４は、例えば、アクチュエータと駆動回路を備える。
【0025】
アームセンサ１０５は、例えば、アーム１０２とエンドエフェクタ１０３の関節に取り付けられているエンコーダ、手先に取り付けられている６軸センサや触覚センサ等である。
【0026】
設定部１０６は、仮想座標原点、仮想作業座標原点を含む仮想絶対座標系を設定する。なお、仮想座標原点、仮想作業座標原点については後述する。
【0027】
カメラ１０７は、例えば、ＣＭＯＳ（Complementary Metal Oxide Semiconductor）撮像素子による撮影装置、またはＣＣＤ（Charge Coupled Device）撮像素子による撮影装置である。なお、カメラ１０７は、深度情報Ｄも得られるＲＧＢ（赤緑青）Ｄカメラであってもよい、
【0028】
カメラ駆動部１０８は、制御指令に含まれるカメラ制御指示に基づいて、カメラ１０７を例えばパン軸方向に傾けさせる。カメラ駆動部１０８は、例えば、アクチュエータと駆動回路を備える。
【0029】
カメラセンサ１０９は、カメラ１０７のパン軸の傾きを検出する。
【0030】
照明１１０は、制御指令に含まれる照明制御指示に基づいて、照明のオン状態とオフ状態、照明の照度、照明の傾き等を変更可能な装置である。
【0031】
スラスタ１１１は、推進力を発生させる装置であり、例えばプロペラとモータ等である。
【0032】
スラスタ駆動部１１２は、制御指令に含まれるスラスタ制御指示に基づいて、スラスタ１１１を駆動する。スラスタ駆動部１１２は、例えば駆動回路を備える。
【0033】
制御部１１３は、制御指令に基づいて、例えば、ロボット本体１０１、アーム１０２、カメラ１０７、スラスタ１１１等の動作を制御する。なお、制御部１１３は、入力部２０１に入力された指制御指令を仮想絶対座標系で制御して、ロボット本体座標系に変換してロボット本体１０１に指令する。
【0034】
記憶部１１４は、ロボット制御装置１００の制御に必要なプログラム、閾値、所定値、数式等を記憶する。記憶部１１４は、例えばロボット本体１０１やアーム１０２の三次元モデルを記憶する。
【0035】
通信部１１６は、操作部２００と情報の送受信を行う。
【0036】
センサ１１７は、例えば、ＤＶＬ（Ｄｏｐｐｌｅｒ Ｖｅｌｏｃｉｔｙ Ｌｏｇ）センサ、Ｉｎｔｅｒｎａｌ ｐｒｅｓｓｕｒｅ（内圧）センサ、慣性計測装置（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）、磁気センサ等である。
【0037】
（センサの役割）
本実施形態では、以下のセンサによって、各情報を取得し、取得した情報に基づいてロボット本体１０１の位置と姿勢を推定する。なお、航行状態に応じて、以下のようにセンサを使い分ける。
・定高度航行（海底面からの高さを一定にする航行）のためのセンサとして、ＤＶＬセンサを用いる。検出値はｚ軸方向（高さ方向、深さ方向）の情報である。
・定深度航行（水面からの深度を一定にする。
航行）のためのセンサとして、Ｉｎｔｅｒｎａｌ ｐｒｅｓｓｕｒｅ（圧力）センサを用いる。検出値はｚ軸方向（高さ方向、深さ方向）の情報である。
・方向維持機能のセンサとして磁気センサ、ＤＶＬセンサを用いる。対象はｚ軸方向の回転θｚである。この場合は、地磁気センサによって北からの回転を検出する。
・位置維持機能のためのセンサとして、慣性計測装置とＤＶＬセンサを用いる。対象はｘ軸方向とｙ軸方向の情報である。この場合は、慣性計測装置の積分値を、例えば１０Ｈｚ程度で積分してドリフト分を補正する。
・姿勢の位置を検出するセンサとして、慣性計測装置とＤＶＬセンサを用いる。対象はｘ軸方向の回転θｘとｙ軸方向の回転θｙである。
本実施形態では、以下のセンサによって、各情報を取得し、取得した情報に基づいてロボット本体１０１の位置と姿勢を推定する。なお、航行状態に応じて、以下のようにセンサを使い分ける。
・定高度航行（海底面からの高さを一定にする航行）のためのセンサとして、ＤＶＬセンサを用いる。検出値はｚ軸方向（高さ方向、深さ方向）の情報である。
・定深度航行（水面からの深度を一定にする。
航行）のためのセンサとして、Ｉｎｔｅｒｎａｌ ｐｒｅｓｓｕｒｅ（圧力）センサを用いる。検出値はｚ軸方向（高さ方向、深さ方向）の情報である。
・方向維持機能のセンサとして磁気センサ、ＤＶＬセンサを用いる。対象はｚ軸方向の回転θｚである。この場合は、地磁気センサによって北からの回転を検出する。
・位置維持機能のためのセンサとして、慣性計測装置とＤＶＬセンサを用いる。対象はｘ軸方向とｙ軸方向の情報である。この場合は、慣性計測装置の積分値を、例えば１０Ｈｚ程度で積分してドリフト分を補正する。
・姿勢の位置を検出するセンサとして、慣性計測装置とＤＶＬセンサを用いる。対象はｘ軸方向の回転θｘとｙ軸方向の回転θｙである。
【0038】
（座標系、原点）
次に、仮想絶対座標系における作業位置の定義と手先の位置・姿勢を説明する。
図４は、絶対座標系、仮想絶対座標系、ロボットの座標系と、ロボット本体と手先の位置・姿勢位置を示す図である。なお、仮想絶対座標系は任意のタイミングでリセット可能であり、任意のタイミングで仮想絶対座標系をリセットする場合、初期状態において仮想絶対座標系とロボット座標系は一致する。その際、仮想原点位置を元にロボットの可動可能エリアを３次元的に定義する。稼働エリアｇ１は、例えば図４のようにＸＹＺ方向の直方体であってもよく、３次元的なポリゴン形状であってもよい。ロボット本体１０１は、可動範囲内において操作者からの指令値に追従するものとした。なお、稼働エリアｇ１内外の判断は、ロボット本体１０１の位置で判断しても良い。また、ロボットの３次元的なポリゴン形状から可動範囲内外を判断しても良い。ロボット本体のアーム１０２の手先の位置と姿勢は、仮想絶対座標系において（ｘ，ｙ，ｚ，θｘ，θｙ，θｚ）で表せる。また、ロボット本体の位置と姿勢は仮想絶対座標系において（ｘｒ，ｙｒ，ｚｒ，θｒｘ，θｒｙ，θｒｚ）で表せる。なお、図４では、エンドエフェクタ１０３、カメラ１０７を省略している。
次に、仮想絶対座標系における作業位置の定義と手先の位置・姿勢を説明する。
図４は、絶対座標系、仮想絶対座標系、ロボットの座標系と、ロボット本体と手先の位置・姿勢位置を示す図である。なお、仮想絶対座標系は任意のタイミングでリセット可能であり、任意のタイミングで仮想絶対座標系をリセットする場合、初期状態において仮想絶対座標系とロボット座標系は一致する。その際、仮想原点位置を元にロボットの可動可能エリアを３次元的に定義する。稼働エリアｇ１は、例えば図４のようにＸＹＺ方向の直方体であってもよく、３次元的なポリゴン形状であってもよい。ロボット本体１０１は、可動範囲内において操作者からの指令値に追従するものとした。なお、稼働エリアｇ１内外の判断は、ロボット本体１０１の位置で判断しても良い。また、ロボットの３次元的なポリゴン形状から可動範囲内外を判断しても良い。ロボット本体のアーム１０２の手先の位置と姿勢は、仮想絶対座標系において（ｘ，ｙ，ｚ，θｘ，θｙ，θｚ）で表せる。また、ロボット本体の位置と姿勢は仮想絶対座標系において（ｘｒ，ｙｒ，ｚｒ，θｒｘ，θｒｙ，θｒｚ）で表せる。なお、図４では、エンドエフェクタ１０３、カメラ１０７を省略している。
【0039】
また、点ｐ１１は、ロボット起動時のグローバル座標系の原点である。点ｐ１２は、仮想絶対座標の仮想原点の位置である。点ｐ１３はロボット本体１０１の原点であり、点ｐ１３ａは移動（回転）前のロボット本体１０１の原点位置、点ｐ１３ｂは移動（回転）後のロボット本体１０１の原点位置である。矢印ｇ２はロボット本体１０１の原点の移動を示す。点ｐ１４はロボット本体１０１にアーム１０２が取り付けられているアーム原点である。点ｐ１４Ｌａは移動（回転）前の左のアーム原点位置、点ｐ１４Ｒａは移動（回転）前の右のアーム原点位置、点ｐ１４Ｌｂは移動（回転）後の左のアーム原点位置、点ｐ１４Ｒｂは移動（回転）後の右のアーム原点位置である。点ｐ１５は手先位置であり、点ｐ１５Ｌは左の手先位置、点ｐ１５Ｒは右の手先位置である。
【0040】
仮想原点位置から見たロボットの現在位置と姿勢を変換行列を用いて求める。また、カメラ１０７のパン軸の位置、手先の位置等を、ロボットの原点位置から見た相対的な現在位置や姿勢として、変換行列を用いて求める。なお、図４のように、ロボット本体１０１の位置と姿勢は、仮想絶対座標の原点からみた相対的なロボットの現在位置と姿勢である。また、カメラ１０７のパン軸の位置と姿勢は、ロボット座標原点（例えばロボット本体１０１の中心位置）から見た相対的な現在位置と姿勢である。また、図４のように、アーム１０１やエンドエフェクタ１０３を動作させロボット本体１０１の移動（回転）の際、ロボット制御装置１００は、手先位置ｐ１５がズレないように制御する。
【0041】
カメラ座標系での手先の位置姿勢の変化指示をδＸｈａｎｄＣＡＭＥＲＡ［ＲＬ］で表す。Ｒは右、Ｌは左を表す。
ロボット座標系での手先の位置姿勢の変化は、上述した変換行列を用いると次式（１）となる。式（１）において行列Ｔは４×４同時変換行列である。姿勢はＥｕｌｅｒ角、Ｑｕａｔｅｒｎｉｏｎであっても良いが、同時変換行列に変換して（１）以降の処理を行う。なお、ＴＣＡＭＥＲＡ→ＲＯＶは、カメラ座標系からロボット座標系への同時変換行列である。なお、カメラ座標系からロボットの絶対座標系への変換行列は、例えばシミュレーションによって予め求めて記憶部１１４に記憶させておく。また、カメラ座標系からロボットの絶対座標系への変換行列は、ロボット座標系原点からのカメラ座標系原点の位置とカメラ１０７のパン軸回転角度により連続的に求めることができる。なお、ロボット座標系からカメラ座標系への変換は、ＰＡＮ軸、ＴＩＬＴ軸、ＲＯＬＥ軸があってもよい。
また、仮想絶対座標系の仮想原点ｐ１２は、（Ｘｐ０，Ｙｐ０，Ｚｐ０，Ｘθ０，Ｙθ０，Ｚθ０）で表される。設定部１０６は、仮想座標原点を任意に位置に設定し、任意の方向、例えば鎖線ｇ４１の方向をｘ軸方向に決定する。なお、仮想座標原点は、ロボット本体１０１の原点、カメラ１０７のパン軸の位置と異なる位置である。また、ロボットの絶対位置は、（Ｘａｃｔ ｐ，Ｙａｃｔ p，Ｚａｃｔ p，Ｘａｃｔ θ，Ｙａｃｔ θ，Ｚａｃｔ θ）で表される。
ロボット座標系での手先の位置姿勢の変化は、上述した変換行列を用いると次式（１）となる。式（１）において行列Ｔは４×４同時変換行列である。姿勢はＥｕｌｅｒ角、Ｑｕａｔｅｒｎｉｏｎであっても良いが、同時変換行列に変換して（１）以降の処理を行う。なお、ＴＣＡＭＥＲＡ→ＲＯＶは、カメラ座標系からロボット座標系への同時変換行列である。なお、カメラ座標系からロボットの絶対座標系への変換行列は、例えばシミュレーションによって予め求めて記憶部１１４に記憶させておく。また、カメラ座標系からロボットの絶対座標系への変換行列は、ロボット座標系原点からのカメラ座標系原点の位置とカメラ１０７のパン軸回転角度により連続的に求めることができる。なお、ロボット座標系からカメラ座標系への変換は、ＰＡＮ軸、ＴＩＬＴ軸、ＲＯＬＥ軸があってもよい。
また、仮想絶対座標系の仮想原点ｐ１２は、（Ｘｐ０，Ｙｐ０，Ｚｐ０，Ｘθ０，Ｙθ０，Ｚθ０）で表される。設定部１０６は、仮想座標原点を任意に位置に設定し、任意の方向、例えば鎖線ｇ４１の方向をｘ軸方向に決定する。なお、仮想座標原点は、ロボット本体１０１の原点、カメラ１０７のパン軸の位置と異なる位置である。また、ロボットの絶対位置は、（Ｘａｃｔ ｐ，Ｙａｃｔ p，Ｚａｃｔ p，Ｘａｃｔ θ，Ｙａｃｔ θ，Ｚａｃｔ θ）で表される。
【0042】
【数1】
【0043】
ロボット座標系でのロボットの位置姿勢の指令の変化は、上述した変換行列を用いると次式（２）となる。
【数2】
【0044】
ロボット起動時に、絶対座標系の位置と姿勢が設定される。例えば、北方向がＸ軸正方向として設定され、陸上で起動した位置が絶対座標系の原点位置として設定される。この場合は、実際に着水して運用する際の座標原点と大きく異なる。このため、本実施形態では、ロボット着水時、あるいは任意のタイミングで仮想絶対座標系を定義する。その際、ロボットの可動範囲を定義することでロボットが想定範囲外に行かないように設定する。任意のタイミングで仮想絶対座標系をリセットする場合は、初期状態において仮想絶対座標系とロボット座標系が一致する。
【0045】
ロボットは、操作者からの指令に応じて、仮想絶対座標系原点から移動する。操作者からの指令がゼロの場合は、ロボットは現在位置を維持するように動作する。
操作者が船上で画像表示部２０２によって見ているカメラ１０７で撮影された画像は、カメラ座標系である。操作者は、カメラ座標系でロボットの位置とアームの位置の指令を行う。カメラ座標系での指令値は、変換行列を用いてロボット本体の座標系に変換される。ロボットの位置と姿勢、アームの位置は、仮想絶対座標系の位置と姿勢に変換され、制御される。そして、ロボット制御装置１００は、ロボット本体の仮想絶対座標系での位置が変化しても、アームの仮想絶対座標系での位置が動かないように制御を行う。
操作者が船上で画像表示部２０２によって見ているカメラ１０７で撮影された画像は、カメラ座標系である。操作者は、カメラ座標系でロボットの位置とアームの位置の指令を行う。カメラ座標系での指令値は、変換行列を用いてロボット本体の座標系に変換される。ロボットの位置と姿勢、アームの位置は、仮想絶対座標系の位置と姿勢に変換され、制御される。そして、ロボット制御装置１００は、ロボット本体の仮想絶対座標系での位置が変化しても、アームの仮想絶対座標系での位置が動かないように制御を行う。
【0046】
仮想絶対座標系でのロボットの位置姿勢の変化は、次式（３）で表せる。なお、ＴＲＯＶ→ＶＩＲＴＵＡＬは、ロボット座標系から仮想絶対座標系への同時変換行列である。
【0047】
【数3】
【0048】
仮想絶対座標系での手先の位置姿勢の変化は、次式（４）で表せる。
【0049】
【数4】
【0050】
グローバル座標系での手先の位置姿勢の変化は、次式（５）で表せる。なお、ＴＶＩＲＴＵＡＬ→ＧＬＯＢＡＬは、仮想絶対座標系からグローバル座標系への同時変換行列である。
【0051】
【数5】
【0052】
グローバル座標系でのロボットの位置姿勢の指令の変化は、次式（６）で表せる。
【0053】
【数6】
【0054】
図５は、仮想座標系の初期状態で、ロボットの可動範囲とロボットの非可動範囲例を示す図である。符号ｇ１０の図は、可動（移動）範囲例と非可動範囲例を示す。符号ｇ２０の図は、可動範囲例、非可動範囲それぞれが球の例を示している。符号ｇ１はロボットの可動範囲、符号ｇ３はロボットの非可動範囲を表す。図５の符号ｇ１０、ｇ２０のように、ロボットの可動範囲ｇ１、非可動範囲ｇ３それぞれは、例えば直方体、直方体、球等の範囲である。点ｇ５は初期の仮想座標系における原点である。符号２０の図において、矢印ｇ２１～ｇ２３は、仮想座標系における各軸方向を示している。可動範囲の直方体の頂点の各座標は、Ｐ１（ｘ１，ｙ１，ｚ１）、Ｐ２（ｘ２，ｙ２，ｚ２）、Ｐ３（ｘ３，ｓ４，ｚ４）、Ｐ４（ｘ４，ｙ４，ｚ４）、Ｐ５（ｘ５，ｙ５，ｚ５）、Ｐ６（ｘ６，ｙ６，ｚ６）、Ｐ７（ｘ７，ｙ７，ｚ７）、Ｐ８（ｘ８，ｙ８，ｚ８）である。本実施形態では、仮想座標系の初期状態で、ロボットに対して、この可動範囲を設定することで、ロボットの可動（移動）範囲を制限する。すなわち、仮想絶対座標系は、このようにロボットの可動（移動）範囲が制限された空間における座標系である。
【0055】
図６は、カメラ座標系での指令の座標変換を説明するための図である。
符号ｐ１６は、カメラ１０７のパン軸の位置である。符号ｇ３１は、カメラ１０７の撮影範囲例である。矢印ｇ３４は、カメラ座標系での移動方向を示している。カメラ座標系での矢印ｇ３４のような横方向への移動指令は、ロボット座標系において前後と左右方向への移動指令として変換される。
点ｐ１７は、仮想作業座標原点（Ｘｗ，Ｙｗ，Ｚｗ，Ｘθｗ，Ｙθｗ，Ｚθｗ）である。設定部１０６は、例えばエンドエフェクタ１０３の作業範囲でカメラ１０７の撮影範囲内に、仮想絶対座標系における仮想作業座標原点を設定する。矢印ｇ３２、ｇ３３、ｇ３４は、ロボット座標系での移動方向への変換を示している。矢印ｇ３２、ｇ３３は、矢印ｇ３４の各方向の成分を表している。なお、仮想作業座標原点は、操作者が画像表示部２０２に表示された画像を視認して決定するようにしてもよい。
符号ｐ１６は、カメラ１０７のパン軸の位置である。符号ｇ３１は、カメラ１０７の撮影範囲例である。矢印ｇ３４は、カメラ座標系での移動方向を示している。カメラ座標系での矢印ｇ３４のような横方向への移動指令は、ロボット座標系において前後と左右方向への移動指令として変換される。
点ｐ１７は、仮想作業座標原点（Ｘｗ，Ｙｗ，Ｚｗ，Ｘθｗ，Ｙθｗ，Ｚθｗ）である。設定部１０６は、例えばエンドエフェクタ１０３の作業範囲でカメラ１０７の撮影範囲内に、仮想絶対座標系における仮想作業座標原点を設定する。矢印ｇ３２、ｇ３３、ｇ３４は、ロボット座標系での移動方向への変換を示している。矢印ｇ３２、ｇ３３は、矢印ｇ３４の各方向の成分を表している。なお、仮想作業座標原点は、操作者が画像表示部２０２に表示された画像を視認して決定するようにしてもよい。
【0056】
制御部１１３は、図６のようにカメラ座標系での制御指令をロボットの絶対座標系に変換して、ロボット本体１０１に指令する。なお、カメラ座標系からロボットの絶対座標系への変換行列は、ロボット座標系の原点からのカメラ座標系の原点の位置とカメラの回転角度により連続的に求められる制御部１１３は、ロボット本体１０１の位置と姿勢を、仮想絶対座標系で制御する。なお、制御部１１３は、次式（７）、（８）により、仮想絶対座標系でのロボット本体１０１の移動に対して、手先位置を仮想絶対座標系で動かないように制御を行う。
【0057】
【数7】
【0058】
【数8】
【0059】
なお、式（７）、（８）において、Ｒは右手先、Ｌは左手先である。また、ｍｏｄｉｆｉｅｄは変換を示している。
【0060】
（処理手順）
次に、ロボット制御システムの処理手順例を説明する。図７は、本実施形態に係るロボット制御システムの処理手順のフローチャートである。なお、以下の処理は、ロボットが作業対象物体に近づいて、作業を行う前提である。
次に、ロボット制御システムの処理手順例を説明する。図７は、本実施形態に係るロボット制御システムの処理手順のフローチャートである。なお、以下の処理は、ロボットが作業対象物体に近づいて、作業を行う前提である。
【0061】
（ステップＳ１）設定部１０６は、作業を開始する前に仮想絶対座標系を設定する。
【0062】
（ステップＳ２）制御部１１３は、作業中、ロボット本体１０１の仮想絶対座標系における位置と姿勢を、センサ検出値を用いて推定する。
【0063】
（ステップＳ３）制御部１１３は、操作者がカメラ座標系でロボット本体１０１やエンドエフェクタ１０３の手先を操作した制御指示を取得する。
【0064】
（ステップＳ４）制御部１１３は、取得したカメラ座標系での制御指令に基づいて、ロボット本体１０１の位置と姿勢を、仮想絶対座標系で制御し、ロボット座標系に変換してロボットに指令する。
【0065】
すなわち、ロボット本体１０１は、ロボット座標系内で入力部２０１によって入力された結果に応じて、ロボット本体１０１を例えば前後左右に動かし、これによるズレを仮想絶対座標系を用いて補正している。なお、仮想絶対座標系の原点は、操作者が決定してもよい。
【0066】
ロボット制御システム１は、ステップＳ２～Ｓ４の処理を作業終了まで繰り返す。
なお、これらの処理は、ロボットの移動時から、アーム１０２やエンドエフェクタ１０３を用いた操作に切り替わった後に行う。
なお、これらの処理は、ロボットの移動時から、アーム１０２やエンドエフェクタ１０３を用いた操作に切り替わった後に行う。
【0067】
以上の処理によって、本実施形態によれば、操作者がカメラ１０７によって撮影された画像を観ながら、ロボット本体１０１と手先を、ロボット本体１０１や手先の位置が変化しても変化しない仮想絶対座標系で操作できる。これにより、本実施形態によれば、操作者が作業操作中にカメラ１０７の画角から対象物を見失うリスクを軽減できるので、操作者がカメラを観ながらロボットと手先を精度良く操作することができる。
【0068】
（処理適用例）
上述した仮想絶対座標系を用いて、例えば以下のように処理を行う。この例では、操作者は、カメラ座標でエンドエフェクタ１０３に対して制御指示を行う。水中灯での作業の場合、アーム１０２やエンドエフェクタ１０３が残土、左右、上下等に移動すると、ロボット本体１０１が影響を受けるため、アーム１０２等の移動に応じて、ロボット本体１０１の制御を制御部１１３が以下のように拘束して行う。
なお、以下のような処理において、制御部１１３は、アーム１０２がロボット本体１０１に接続される点近傍に設定されるアーム原点と、アームの先端であるアーム先端部の位置をそれぞれ取得する。そして制御部１１３は、アーム原点とアーム先端部の間に設定される拘束モデルを定義し、アーム原点とアーム先端部の距離の変化に応じて発生する拘束力を拘束モデルから算出し、拘束力の値から前記ロボット本体の位置指令を算出する。
上述した仮想絶対座標系を用いて、例えば以下のように処理を行う。この例では、操作者は、カメラ座標でエンドエフェクタ１０３に対して制御指示を行う。水中灯での作業の場合、アーム１０２やエンドエフェクタ１０３が残土、左右、上下等に移動すると、ロボット本体１０１が影響を受けるため、アーム１０２等の移動に応じて、ロボット本体１０１の制御を制御部１１３が以下のように拘束して行う。
なお、以下のような処理において、制御部１１３は、アーム１０２がロボット本体１０１に接続される点近傍に設定されるアーム原点と、アームの先端であるアーム先端部の位置をそれぞれ取得する。そして制御部１１３は、アーム原点とアーム先端部の間に設定される拘束モデルを定義し、アーム原点とアーム先端部の距離の変化に応じて発生する拘束力を拘束モデルから算出し、拘束力の値から前記ロボット本体の位置指令を算出する。
【0069】
（１）第１の拘束条件（アーム１０２を前後に伸び縮みさせた場合のロボット本体１０１の前後の移動（ｘ軸方向）指令を決定する拘束）
次に、ロボット本体１０１の前後の移動（ｘ軸方向）指令を決定する拘束について、さらに説明する。図８は、ロボット本体の前後の移動（ｘ軸方向）指令を決定する拘束を説明するための図である。
この場合は、アーム１０２の原点と手先との間に拘束モデルを設定する。なお、拘束モデルは、例えば筋肉モデルまたはポテンシャル法によるモデルである。
次に、ロボット本体１０１の前後の移動（ｘ軸方向）指令を決定する拘束について、さらに説明する。図８は、ロボット本体の前後の移動（ｘ軸方向）指令を決定する拘束を説明するための図である。
この場合は、アーム１０２の原点と手先との間に拘束モデルを設定する。なお、拘束モデルは、例えば筋肉モデルまたはポテンシャル法によるモデルである。
【0070】
符号ｇ１００、ｇ１２０の図は、アーム１０２をロボット本体１０１に近づけ縮めた状態を示している。符号ｇ１１０、ｇ１３０の図は、アーム１０２をロボット本体１０１から遠ざけて伸ばした状態を示している。
符号ｇ１２２は、アーム１０２が縮んだ状態ｇ１２１のｘ軸方向の拘束力を抽出した結果を示す。符号１３２は、アーム１０２が伸びた状態ｇ１３１のｘ軸方向の拘束力を抽出した結果を示す。
符号ｇ１２２は、アーム１０２が縮んだ状態ｇ１２１のｘ軸方向の拘束力を抽出した結果を示す。符号１３２は、アーム１０２が伸びた状態ｇ１３１のｘ軸方向の拘束力を抽出した結果を示す。
【0071】
符号ｇ１２２、ｇ１３２、さらに不感帯ｇ１４１を用いて、第１の拘束条件であるｘ軸方向の拘束力は、符号ｇ１４０のように表される。なお、不感帯では、ロボット本体１０１が動かないとする。なお、曲線の部分ｇ１４２とｇ１４３が、拘束力が働く領域である。曲線の理由は、筋肉モデルの場合、バネとダンパーで表せるため直線的に動作できないためであるのと、曲線の方が動作を自然にできるあるためである。また、拘束力が曲線の理由は、入力部２０１が例えばゲームコントローラのようにボタンやジョイスティックで構成されるため、拘束力を挑戦としてしまうと、わずかな操作でも急激な動作をしてしまうためでもある。
【0072】
これにより、例えば以下のように制御される。
Ｉ．右アーム１０２Ｒが伸展、左アーム１０２Ｌが不感帯の状態；ロボット本体１０１が前進
ＩＩ．右アーム１０２Ｒが不感帯、左アーム１０２Ｌが伸展の状態；ロボット本体１０１が前進
ＩＩＩ．左右のアーム１０２が伸展の状態；ロボット本体１０１が前進大
ＩＶ．右アーム１０２Ｒが収縮、左アーム１０２Ｌが不感帯の状態；ロボット本体１０１が後進
Ｖ．右アーム１０２Ｒが不感帯、左アーム１０２Ｌが収縮；ロボット本体１０１が後進
ＶＩ．左右のアーム１０２が収縮；ロボット本体１０１が後進大
Ｉ．右アーム１０２Ｒが伸展、左アーム１０２Ｌが不感帯の状態；ロボット本体１０１が前進
ＩＩ．右アーム１０２Ｒが不感帯、左アーム１０２Ｌが伸展の状態；ロボット本体１０１が前進
ＩＩＩ．左右のアーム１０２が伸展の状態；ロボット本体１０１が前進大
ＩＶ．右アーム１０２Ｒが収縮、左アーム１０２Ｌが不感帯の状態；ロボット本体１０１が後進
Ｖ．右アーム１０２Ｒが不感帯、左アーム１０２Ｌが収縮；ロボット本体１０１が後進
ＶＩ．左右のアーム１０２が収縮；ロボット本体１０１が後進大
【0073】
（２）第２の拘束条件（ロボット本体１０１を左右の移動（ｙ軸方向）指令を決定する拘束）
次に、ロボット本体１０１の左右の移動（ｙ軸方向）指令を決定する拘束について、さらに説明する。図９は、ロボット本体の左右の移動（ｙ軸方向）指令を決定する拘束を説明するための図である。
この場合は、アーム１０２の原点と手先位置との間に拘束モデルを設定する。
次に、ロボット本体１０１の左右の移動（ｙ軸方向）指令を決定する拘束について、さらに説明する。図９は、ロボット本体の左右の移動（ｙ軸方向）指令を決定する拘束を説明するための図である。
この場合は、アーム１０２の原点と手先位置との間に拘束モデルを設定する。
【0074】
符号ｇ２００の図は、アーム１０２を左側に移動させた状態を示している。符号ｇ２１の図は、アーム１０２を右側に移動させた状態を示している。
符号ｇ２０２は、アーム１０２が左に移動した状態ｇ２０１のｙ軸方向の拘束力を抽出した結果を示す。符号ｇ２１２は、アーム１０２が右に移動した状態ｇ２１１のｙ軸方向の拘束力を抽出した結果を示す。
符号ｇ２０２は、アーム１０２が左に移動した状態ｇ２０１のｙ軸方向の拘束力を抽出した結果を示す。符号ｇ２１２は、アーム１０２が右に移動した状態ｇ２１１のｙ軸方向の拘束力を抽出した結果を示す。
【0075】
符号ｇ２０２、ｇ２１２、不感帯ｇ２２１を用いて、第２の拘束条件であるｘ軸方向の拘束力は、符号ｇ２２０のように表される。なお、不感帯では、ロボット本体１０１が動かないとする。
【0076】
これにより、例えば以下のように制御される。
Ｉ．右アーム１０２Ｒが右に移動、左アーム１０２Ｌが不感帯の状態；ロボット本体１０１が右に移動
ＩＩ．右アーム１０２Ｒが不感帯、左アーム１０２Ｌが左に移動の状態；ロボット本体１０１が左に移動
ＩＩＩ．左右アーム１０２が肩の間の状態；ロボット本体１０１がｙ軸方向で停止
ＩＶ． 右アーム１０２Ｒが右に移動、左アーム１０２Ｌが左に移動の状態；左右の拘束力の大小に応じてロボット本体１０１の左右いずれかの移動が決まる
Ｉ．右アーム１０２Ｒが右に移動、左アーム１０２Ｌが不感帯の状態；ロボット本体１０１が右に移動
ＩＩ．右アーム１０２Ｒが不感帯、左アーム１０２Ｌが左に移動の状態；ロボット本体１０１が左に移動
ＩＩＩ．左右アーム１０２が肩の間の状態；ロボット本体１０１がｙ軸方向で停止
ＩＶ． 右アーム１０２Ｒが右に移動、左アーム１０２Ｌが左に移動の状態；左右の拘束力の大小に応じてロボット本体１０１の左右いずれかの移動が決まる
【0077】
（３）第３の拘束条件（ロボット本体１０１を上下の移動（ｚ軸方向）指令を決定する拘束）
次に、ロボット本体１０１の上下の移動（ｚ軸方向）指令を決定する拘束について、さらに説明する。図１０は、ロボット本体の上下の移動（ｚ軸方向）指令を決定する拘束を説明するための図である。
この場合は、アーム１０２の原点と手先位置との間に拘束モデルを設定する。
次に、ロボット本体１０１の上下の移動（ｚ軸方向）指令を決定する拘束について、さらに説明する。図１０は、ロボット本体の上下の移動（ｚ軸方向）指令を決定する拘束を説明するための図である。
この場合は、アーム１０２の原点と手先位置との間に拘束モデルを設定する。
【0078】
符号ｇ３００の図は、アーム１０２を上側に移動させた状態を示している。符号ｇ３１０の図は、アーム１０２を下側に移動させた状態を示している。
符号ｇ３０２は、アーム１０２が上側に移動した状態ｇ３０１のｚ軸方向の拘束力を抽出した結果を示す。符号ｇ３１２は、アーム１０２が下側に移動した状態ｇ３１１のｚ軸方向の拘束力を抽出した結果を示す。
符号ｇ３０２は、アーム１０２が上側に移動した状態ｇ３０１のｚ軸方向の拘束力を抽出した結果を示す。符号ｇ３１２は、アーム１０２が下側に移動した状態ｇ３１１のｚ軸方向の拘束力を抽出した結果を示す。
【0079】
符号ｇ３０２、ｇ３１２、不感帯ｇ３２１を用いて、第３の拘束条件であるｚ軸方向の拘束力は、符号ｇ３２０のように表される。なお、不感帯では、ロボット本体１０１が動かないとする。
【0080】
なお、制御部１１３は、ロボット本体１０１への指令を、左右の拘束力を足してコンプライアンス制御で変化分を指令とする。
【0081】
これにより、例えば以下のように制御される。
Ｉ．右アーム１０２Ｒが上に移動、左アーム１０２Ｌが不感帯の状態；ロボット本体１０１が上方向に移動
ＩＩ．右アーム１０２Ｒが不感帯、左アーム１０２Ｌが上に移動の状態；ロボット本体１０１が上方向に移動
ＩＩＩ．左右アーム１０２が上に移動の状態；ロボット本体１０１が上方向に移動大
ＩＶ．左右アーム１０２が不感帯の状態；ロボット本体１０１が静止
Ｖ．右アーム１０２Ｒが下、左アーム１０２Ｌが不感帯の状態；ロボット本体１０１が下方向に移動
ＶＩ．右アーム１０２Ｒが不感帯、左アーム１０２Ｌが下の状態；ロボット本体１０１が下方向に移動
ＶＩＩ．左右アーム１０２が下の状態；ロボット本体１０１が下方向に移動大
Ｉ．右アーム１０２Ｒが上に移動、左アーム１０２Ｌが不感帯の状態；ロボット本体１０１が上方向に移動
ＩＩ．右アーム１０２Ｒが不感帯、左アーム１０２Ｌが上に移動の状態；ロボット本体１０１が上方向に移動
ＩＩＩ．左右アーム１０２が上に移動の状態；ロボット本体１０１が上方向に移動大
ＩＶ．左右アーム１０２が不感帯の状態；ロボット本体１０１が静止
Ｖ．右アーム１０２Ｒが下、左アーム１０２Ｌが不感帯の状態；ロボット本体１０１が下方向に移動
ＶＩ．右アーム１０２Ｒが不感帯、左アーム１０２Ｌが下の状態；ロボット本体１０１が下方向に移動
ＶＩＩ．左右アーム１０２が下の状態；ロボット本体１０１が下方向に移動大
【0082】
（４）第４の拘束条件（ロボット本体１０１の回転（θＺ）指令を決定する拘束状態である。）
次に、ロボット本体１０１の回転移動（ｚ軸回転）指令を決定する拘束について、さらに説明する。図１１は、ロボット本体の回転移動（ｚ軸回転）指令を決定する拘束を説明するための図である。
次に、ロボット本体１０１の回転移動（ｚ軸回転）指令を決定する拘束について、さらに説明する。図１１は、ロボット本体の回転移動（ｚ軸回転）指令を決定する拘束を説明するための図である。
【0083】
符号ｇ４００の図は、アーム１０２を回転移動させた状態を示している。
符号ｇ４０１は、アーム１０２が一番短い状態を表し、符号ｇ４０２は、アーム１０２が一番長い状態（伸ばしきった状態）を表している。
符号ｇ４０１は、アーム１０２が一番短い状態を表し、符号ｇ４０２は、アーム１０２が一番長い状態（伸ばしきった状態）を表している。
【0084】
符号ｇ４０１、ｇ４０２、不感帯ｇ４１１を用いて、第４の拘束条件である回転方向の拘束力は、符号ｇ４１０のように表される。ここで、不感帯の開始位置をＸｃｏｎｔ＿ｍｉｎとし、不感帯の終了位置をＸｃｏｎｔ＿ｍａｘとする。
【0085】
これにより、例えば以下のように制御される。
Ｉ．右アーム１０２Ｒが伸展、左アーム１０２Ｌが収縮の状態；ロボット本体１０１が反時計回りに回転
ＩＩ．右アーム１０２Ｒが収縮、左アーム１０２Ｌが伸展の状態；ロボット本体１０１が時計回りに回転
Ｉ．右アーム１０２Ｒが伸展、左アーム１０２Ｌが収縮の状態；ロボット本体１０１が反時計回りに回転
ＩＩ．右アーム１０２Ｒが収縮、左アーム１０２Ｌが伸展の状態；ロボット本体１０１が時計回りに回転
【0086】
なお、以下はオプション扱いであってもよい。
ＩＩＩ．右アーム１０２Ｒが伸展、左アーム１０２Ｌが不感帯の状態；ロボット本体１０１が反時計回りに回転
ＩＶ．右アーム１０２Ｒが不感帯、左アーム１０２Ｌが伸展の状態；ロボット本体１０１が時計回りに回転
ＩＩＩ．右アーム１０２Ｒが伸展、左アーム１０２Ｌが不感帯の状態；ロボット本体１０１が反時計回りに回転
ＩＶ．右アーム１０２Ｒが不感帯、左アーム１０２Ｌが伸展の状態；ロボット本体１０１が時計回りに回転
【0087】
（第４の拘束条件の第１変形例）
以下では、目的に応じて回転中心の設定を選択する例を説明する。
図１２は、回転中心がロボット本体の原点の場合の例を示す図である。
点ｇ５０１は、ロボット本体の原点である。四角ｇ５０２は、回転前のロボット本体１０１の外観の姿勢である。四角ｇ５０３は、回転後のロボット本体１０１の外観の姿勢である。
矢印ｇ５１１は、ロボット本体１０１の原点における回転前のｘ軸方向の成分である。矢印ｇ５１２は、ロボット本体１０１の原点における回転前のｙ軸方向の成分である。
矢印ｇ５１３は、ロボット本体１０１の原点における回転後のｘ軸方向の成分である。矢印ｇ５１４は、ロボット本体１０１の原点における回転後のｙ軸方向の成分である。
以下では、目的に応じて回転中心の設定を選択する例を説明する。
図１２は、回転中心がロボット本体の原点の場合の例を示す図である。
点ｇ５０１は、ロボット本体の原点である。四角ｇ５０２は、回転前のロボット本体１０１の外観の姿勢である。四角ｇ５０３は、回転後のロボット本体１０１の外観の姿勢である。
矢印ｇ５１１は、ロボット本体１０１の原点における回転前のｘ軸方向の成分である。矢印ｇ５１２は、ロボット本体１０１の原点における回転前のｙ軸方向の成分である。
矢印ｇ５１３は、ロボット本体１０１の原点における回転後のｘ軸方向の成分である。矢印ｇ５１４は、ロボット本体１０１の原点における回転後のｙ軸方向の成分である。
【0088】
この場合のコマンドは、成分の大きさの変化がなく回転方向のみ変化する。
このため、ｘ軸方向のコマンドＸｃｍｄはＸｃｕｒｒｅｎｔ（現在値と同じ）、ｙ軸方向のコマンドＹｃｍｄはＹｃｕｒｒｅｎｔ（現在値と同じ）である。
このため、ｘ軸方向のコマンドＸｃｍｄはＸｃｕｒｒｅｎｔ（現在値と同じ）、ｙ軸方向のコマンドＹｃｍｄはＹｃｕｒｒｅｎｔ（現在値と同じ）である。
【0089】
図１３は、回転中心がカメラのパン軸の場合の例を示す図である。
点ｇ５２１は、カメラ１０７のパン軸である。点ｇ５２２は、回転前のロボット本体の原点である。点ｇ５２３は、回転後のロボット本体の原点である。四角ｇ５２４は、回転前のロボット本体１０１の外観の姿勢である。四角ｇ５２５は、回転後のロボット本体１０１の外観の姿勢である。
点ｇ５２１は、カメラ１０７のパン軸である。点ｇ５２２は、回転前のロボット本体の原点である。点ｇ５２３は、回転後のロボット本体の原点である。四角ｇ５２４は、回転前のロボット本体１０１の外観の姿勢である。四角ｇ５２５は、回転後のロボット本体１０１の外観の姿勢である。
【0090】
矢印ｇ５３１は、ロボット本体１０１の原点における回転前のｘ軸方向の成分である。矢印ｇ５３２は、ロボット本体１０１の原点における回転前のｙ軸方向の成分である。
矢印ｇ５３５は、ロボット本体１０１の原点における回転後のｘ軸方向の成分である。矢印ｇ５３６は、ロボット本体１０１の原点における回転後のｙ軸方向の成分である。
矢印ｇ５３５は、ロボット本体１０１の原点における回転後のｘ軸方向の成分である。矢印ｇ５３６は、ロボット本体１０１の原点における回転後のｙ軸方向の成分である。
【0091】
矢印ｇ５３３は、カメラ１０７のパン軸における回転前のｘ軸方向の成分である。矢印ｇ５３４は、カメラ１０７のパン軸の原点における回転前のｙ軸方向の成分である。
矢印ｇ５３７は、カメラ１０７のパン軸における回転後のｘ軸方向の成分である。矢印ｇ５３８は、カメラ１０７のパン軸における回転後のｙ軸方向の成分である。
矢印ｇ５３７は、カメラ１０７のパン軸における回転後のｘ軸方向の成分である。矢印ｇ５３８は、カメラ１０７のパン軸における回転後のｙ軸方向の成分である。
【0092】
カメラ１０７のパン軸とロボット本体１０１とのｘ軸方向の距離をＤｃｘ、回転成分の角度をθとすると、ｘ軸方向のコマンドＸｃｍｄはＸｃｕｒｒｅｎｔ＋Ｄｃｘ＊（１－ｃｏｓ（θ））、ｙ軸方向のコマンドＹｃｍｄはＹｃｕｒｒｅｎｔ＋Ｄｃｘ＊ｓｉｎ（θ）である。
【0093】
図１４は、回転中心が仮想的作業位置の場合の例を示す図である。
点ｇ５４１は、仮想的作業位置である。点ｇ５４２は、回転前のロボット本体の原点である。点ｇ５４３は、回転後のロボット本体の原点である。四角ｇ５４４は、回転前のロボット本体１０１の外観の姿勢である。四角ｇ５４５は、回転後のロボット本体１０１の外観の姿勢である。
点ｇ５４１は、仮想的作業位置である。点ｇ５４２は、回転前のロボット本体の原点である。点ｇ５４３は、回転後のロボット本体の原点である。四角ｇ５４４は、回転前のロボット本体１０１の外観の姿勢である。四角ｇ５４５は、回転後のロボット本体１０１の外観の姿勢である。
【0094】
矢印ｇ５５１は、ロボット本体１０１の原点における回転前のｘ軸方向の成分である。矢印ｇ５５２は、ロボット本体１０１の原点における回転前のｙ軸方向の成分である。
矢印ｇ５５５は、ロボット本体１０１の原点における回転後のｘ軸方向の成分である。矢印ｇ５５６は、ロボット本体１０１の原点における回転後のｙ軸方向の成分である。
矢印ｇ５５５は、ロボット本体１０１の原点における回転後のｘ軸方向の成分である。矢印ｇ５５６は、ロボット本体１０１の原点における回転後のｙ軸方向の成分である。
【0095】
矢印ｇ５５３は、仮想的作業位置における回転前のｘ軸方向の成分である。矢印ｇ５５４は、仮想的作業位置における回転前のｙ軸方向の成分である。
矢印ｇ５５７は、仮想的作業位置における回転後のｘ軸方向の成分である。矢印ｇ５５８は、仮想的作業位置における回転後のｙ軸方向の成分である。
矢印ｇ５５７は、仮想的作業位置における回転後のｘ軸方向の成分である。矢印ｇ５５８は、仮想的作業位置における回転後のｙ軸方向の成分である。
【0096】
仮想的作業位置とロボット本体１０１とのｘ軸方向の距離をＤｗｘ、回転成分の角度をθとすると、ｘ軸方向のコマンドＸｃｍｄはＸｃｕｒｒｅｎｔ＋Ｄｗｘ＊（１－ｃｏｓ（θ））、ｙ軸方向のコマンドＹｃｍｄはＹｃｕｒｒｅｎｔ＋Ｄｗｘ＊ｓｉｎ（θ）である。
【0097】
図１５は、回転中心が左アームの手先位置の場合の例を示す図である。
点ｇ５６１は、左アーム１０２Ｌの手先位置である。点ｇ５６２は、回転前のロボット本体の原点である。点ｇ５６３は、回転後のロボット本体の原点である。四角ｇ５６４は、回転前のロボット本体１０１の外観の姿勢である。四角ｇ５６５は、回転後のロボット本体１０１の外観の姿勢である。
点ｇ５６１は、左アーム１０２Ｌの手先位置である。点ｇ５６２は、回転前のロボット本体の原点である。点ｇ５６３は、回転後のロボット本体の原点である。四角ｇ５６４は、回転前のロボット本体１０１の外観の姿勢である。四角ｇ５６５は、回転後のロボット本体１０１の外観の姿勢である。
【0098】
矢印ｇ５７１は、ロボット本体１０１の原点における回転前のｘ軸方向の成分である。矢印ｇ５７２は、ロボット本体１０１の原点における回転前のｙ軸方向の成分である。
矢印ｇ５７５は、ロボット本体１０１の原点における回転後のｘ軸方向の成分である。矢印ｇ５７６は、ロボット本体１０１の原点における回転後のｙ軸方向の成分である。
矢印ｇ５７５は、ロボット本体１０１の原点における回転後のｘ軸方向の成分である。矢印ｇ５７６は、ロボット本体１０１の原点における回転後のｙ軸方向の成分である。
【0099】
矢印ｇ５７３は、左アーム１０２Ｌの手先位置における回転前のｘ軸方向の成分である。矢印ｇ５７４は、左アーム１０２Ｌの手先位置における回転前のｙ軸方向の成分である。
矢印ｇ５７７は、左アーム１０２Ｌの手先位置における回転後のｘ軸方向の成分である。矢印ｇ５７８は、左アーム１０２Ｌの手先位置における回転後のｙ軸方向の成分である。
矢印ｇ５７７は、左アーム１０２Ｌの手先位置における回転後のｘ軸方向の成分である。矢印ｇ５７８は、左アーム１０２Ｌの手先位置における回転後のｙ軸方向の成分である。
【0100】
左アーム１０２Ｌの手先位置とロボット本体１０１とのｘ軸方向の距離をＤｈｘ、回転成分の角度をθとすると、ｘ軸方向のコマンドＸｃｍｄはＸｃｕｒｒｅｎｔ＋Ｄｈｘ＊（１－ｃｏｓ（θ））、ｙ軸方向のコマンドＹｃｍｄはＹｃｕｒｒｅｎｔ＋Ｄｈｘ＊ｓｉｎ（θ）である。
【0101】
（第４の拘束条件の第２変形例）
ロボット本体１０１にカメラ１０７が取り付けられている場合は、図１６のように、アームの移動に応じて、ロボット本体１０１の原点ｇ６０１を回転中心に回転させると撮影範囲が符号ｇ６０２から符号ｇ６０３のようにズレる。なお、四角ｇ６０４は、回転前のロボット本体１０１の外観の姿勢である。四角ｇ６０５は、回転後のロボット本体１０１の外観の姿勢である。図１６は、ロボット本体の原点を回転中心に回転させた場合の撮影範囲がズレる例を示す図である。
ロボット本体１０１にカメラ１０７が取り付けられている場合は、図１６のように、アームの移動に応じて、ロボット本体１０１の原点ｇ６０１を回転中心に回転させると撮影範囲が符号ｇ６０２から符号ｇ６０３のようにズレる。なお、四角ｇ６０４は、回転前のロボット本体１０１の外観の姿勢である。四角ｇ６０５は、回転後のロボット本体１０１の外観の姿勢である。図１６は、ロボット本体の原点を回転中心に回転させた場合の撮影範囲がズレる例を示す図である。
【0102】
このため、撮影中の制御の場合は、図１７のようにカメラ１０７のパン軸ｇ６２１を中心に回転するようにする。図１７は、カメラのパン軸を回転中心に回転させた場合の撮影範囲例を示す図である。点ｇ６２２は、回転前のロボット本体１０１の原点位置である。点ｇ６２３は、回転後のロボット本体１０１の原点位置である。符号ｇ６２４は、回転前後の撮影範囲である。四角ｇ６２５は、回転前のロボット本体１０１の外観の姿勢である。四角ｇ６２６は、回転後のロボット本体１０１の外観の姿勢である。
【0103】
第２変形例では、ロボット本体１０１の原点中心で回転させた場合に撮影範囲がズレる例を説明したが、図１８のように仮想的作業位置ｇ６４２を設定しロボット本体１０１の原点ｇ６４１を中心に回転させた場合に撮影範囲が符号ｇ６４３から符号ｇ６４４のようにズレる。図１８は、仮想的作業位置を設定しロボット本体の原点を中心に回転させた場合に撮影範囲がズレる例を示す図である。
【0104】
このような場合は、図１９のように仮想的作業位置ｇ６６１を中心に回転させることで、符号ｇ６６４から符号ｇ６６５のように視野の変化を低減することができる。点ｇ６６２は、回転前のロボット本体１０１の原点位置である。点ｇ６６３は、回転後のロボット本体１０１の原点位置である。図１９は、仮想的作業位置を中心に回転させた場合の視野の例を示す図である。
なお、ｘ軸方向のコマンドＸｃｍｄはＸｃｕｒｒｅｎｔ＋Ｄｗｘ＊（１－ｃｏｓ（θ））、ｙ軸方向のコマンドＹｃｍｄはＹｃｕｒｒｅｎｔ＋Ｄｗｘ＊ｓｉｎ（θ）である。
なお、ｘ軸方向のコマンドＸｃｍｄはＸｃｕｒｒｅｎｔ＋Ｄｗｘ＊（１－ｃｏｓ（θ））、ｙ軸方向のコマンドＹｃｍｄはＹｃｕｒｒｅｎｔ＋Ｄｗｘ＊ｓｉｎ（θ）である。
【0105】
【0106】
なお、本発明におけるロボット制御装置１００の機能の全てまたは一部を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することによりロボット制御装置１００が行う処理の全てまたは一部を行ってもよい。なお、ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。また、「コンピュータシステム」は、ホームページ提供環境（あるいは表示環境）を備えたＷＷＷシステムも含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ－ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリ（ＲＡＭ）のように、一定時間プログラムを保持しているものも含むものとする。
【0107】
また、上記プログラムは、このプログラムを記憶装置等に格納したコンピュータシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピュータシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。また、上記プログラムは、前述した機能の一部を実現するためのものであってもよい。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。
【0108】
以上、本発明を実施するための形態について実施形態を用いて説明したが、本発明はこうした実施形態に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形および置換を加えることができる。
【符号の説明】
【0109】
１…ロボット制御システム、１００…ロボット制御装置、２００…操作部、１０１…ロボット本体、１０２，１０２Ｌ，１０２Ｒ…アーム、１０３…エンドエフェクタ、１０４…駆動部、１０５…アームセンサ、１０６…設定部、１０７…カメラ、１０８…カメラ駆動部、１０９…カメラセンサ、１１０…照明、１１１…スラスタ、１１２…スラスタ駆動部、１１３…制御部、１１４…記憶部、１１６…通信部、１１７…センサ、２０１…入力部、２０２…画像表示部、２０３…通信部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】