特開 2023-160099 遠隔作業ロボットの制御装置、および、制御方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2023160099

(43)【公開日】2023-11-02

(54)【発明の名称】遠隔作業ロボットの制御装置、および、制御方法

(51)【国際特許分類】

   B25J 13/08 20060101AFI20231026BHJP

   B25J 3/00 20060101ALI20231026BHJP

【ＦＩ】

B25J13/08 A

B25J3/00 Z

【審査請求】未請求

【請求項の数】6

【出願形態】ＯＬ

(21)【出願番号】P 2022070185

(22)【出願日】2022-04-21

(71)【出願人】

【識別番号】507250427

【氏名又は名称】日立ＧＥニュークリア・エナジー株式会社

(74)【代理人】

【識別番号】110000350

【氏名又は名称】ポレール弁理士法人

(72)【発明者】

【氏名】佐藤 義人

(72)【発明者】

【氏名】村田 幸弘

(72)【発明者】

【氏名】上野 克宜

(72)【発明者】

【氏名】平野 克彦

【テーマコード（参考）】

3C707

【Ｆターム（参考）】

3C707AS27

3C707BS10

3C707JT04

3C707JU12

3C707KS03

3C707KS16

3C707KS20

3C707KS36

3C707KT03

3C707KT05

3C707KT11

3C707KV18

3C707LS15

3C707LU01

3C707MT02

(57)【要約】

【課題】 センサ位置取得処理と各関節の状態量演算処理の完了までに要する時間を、遠隔作業ロボットの制御周期内に抑制しつつ、遠隔作業ロボットの各関節の最大速度を高める。
【解決手段】 関節動作前の第一時刻におけるセンサ情報から検出した第一特徴点の位置と、関節動作後の第二時刻におけるセンサ情報から検出した第二特徴点の位置を比較して、センサ情報上での特徴点の移動量を求めることで、センサの姿勢の変化と、関節の状態量の変化を推定するセンサ姿勢検出部と、センサから第一特徴点までの距離と、動作させた関節とセンサの相対配置と、動作させた関節の動作方向と、に基づいて、第一時刻から第二時刻までの特徴点のセンサ情報上での移動量が、予め定めた閾値を超えない最大速度を求める関節速度制限部と、関節速度制限部が求めた最大速度の範囲内で関節を動作させるロボット制御部と、を有する制御装置とした。
【選択図】図１

【特許請求の範囲】

【請求項1】

２個以上のリンクを連結する関節を備えた遠隔作業ロボットを、何れかのリンクに設置したセンサが認識したセンサ情報に基づいて制御する制御装置であって、
関節動作前の第一時刻におけるセンサ情報から検出した第一特徴点の位置と、関節動作後の第二時刻におけるセンサ情報から検出した第二特徴点の位置を比較して、センサ情報上での特徴点の移動量を求めることで、前記センサの姿勢の変化と、前記関節の状態量の変化を推定するセンサ姿勢検出部と、
前記センサから前記第一特徴点までの距離と、動作させた関節と前記センサの相対配置と、動作させた関節の動作方向と、に基づいて、前記第一時刻から前記第二時刻までの前記特徴点の前記センサ情報上での移動量が、予め定めた閾値を超えない最大速度を求める関節速度制限部と、
該関節速度制限部が求めた前記最大速度の範囲内で前記関節を動作させるロボット制御部と、
を有することを特徴とする制御装置。

【請求項2】

請求項１に記載の制御装置において、
前記センサは、ステレオカメラ、または、深度カメラであり、
前記センサ姿勢検出部は、前記センサ情報に含まれる画像情報を用いて、前記特徴点を検出することを特徴とする制御装置。

【請求項3】

請求項１に記載の制御装置において、
前記センサは、２軸の超音波センサ、または、２軸のレーザセンサであり、
前記センサ姿勢検出部は、前記センサ情報に含まれる奥行情報を用いて、前記特徴点を検出することを特徴とする制御装置。

【請求項4】

請求項１に記載の制御装置において、
前記関節は、回転関節、または、直動関節であり、かつ、一つ以上の前記回転関節を含むことを特徴とする制御装置。

【請求項5】

請求項１に記載の制御装置において、
前記閾値は、第一時刻に検出したセンサ情報内の第一特徴点と、第二時刻に検出したとして扱われる仮想的なセンサ情報内の第二特徴点と、に基づいて、前記センサの姿勢の変化と前記関節の状態量の変化を推定する処理に要する時間が、前記遠隔作業ロボットの制御周期以下となるように前記第二特徴点の位置を設定したときの、前記第一特徴点から前記第二特徴点までの移動量であることを特徴とする制御装置。

【請求項6】

２個以上のリンクを連結する関節を備えた遠隔作業ロボットを、何れかのリンクに設置したセンサが認識したセンサ情報に基づいて制御する制御方法であって、
関節動作前の第一時刻におけるセンサ情報から検出した第一特徴点の位置と、関節動作後の第二時刻におけるセンサ情報から検出した第二特徴点の位置を比較して、センサ情報上での特徴点の移動量を求めることで、前記センサの姿勢の変化と、前記関節の状態量の変化を推定するセンサ姿勢検出ステップと、
前記センサから前記第一特徴点までの距離と、動作させた関節と前記センサの相対配置と、動作させた関節の動作方向と、に基づいて、前記第一時刻から前記第二時刻までの前記特徴点の前記センサ情報上での移動量が、予め定めた閾値を超えない最大速度を求める関節速度制限ステップと、
該関節速度制限ステップで求めた前記最大速度の範囲内で前記関節を動作させるロボット制御ステップと、
を有することを特徴とする制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、過酷環境下で使用される遠隔作業ロボットを遠隔制御する、制御装置、および、制御方法に関する。

【背景技術】

【0002】

過酷環境下で使用される遠隔作業ロボットの手先位置や各関節の状態量を検出し、遠隔作業ロボットの動作を適切に遠隔制御する従来技術として、特許文献１の作業システムが知られている。

【0003】

例えば、同文献の要約書には、「複数個のリンク１１～１３が複数個の関節１４，１５で連続的に連結されているアーム装置２と、アーム装置２の最先端のリンク１１に設けられた画像センサ２１と、アーム装置２の駆動を制御する駆動制御部と、駆動制御部によってアーム装置２の最も先端側からｉ番目の関節の動きを固定してアーム装置２の最も先端側からｉ＋１番目の関節を駆動しながら画像センサ２１で所定の対象物を複数回撮像する撮像部と、撮像部で撮像した各画像を用い対象物を基準とした座標系での前記各画像における前記画像センサの位置を求める座標取得部と、座標取得部で求めた画像センサ２１の位置に基づいてアーム装置２の最も先端側からｉ番目の関節の関節状態量を求める関節状態量判定部とを備えている。」と記載されている。

【0004】

このように、特許文献１では、画像センサが撮像した画像に基づいて、画像センサの位置を検出するとともに、アーム装置の関節状態量を検出する技術が開示されている。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特開２０２１－１７１８８２号公報

【発明の概要】

【発明が解決しようとする課題】

【0006】

特許文献１の作業システムでは、アーム装置のリンクを高速で動かすと、リンクに設けた画像センサも高速で動き、連続撮像した各画像内では撮像対象物の移動量が大きくなる。そのため、連続撮像した各画像内の撮像対象物を追跡するには、各画像内でのテンプレートマッチングの対象領域をリンク速度に略比例させて拡大する必要がある。

【0007】

このことは、リンク速度に略比例して、撮像対象物の追跡処理に要するテンプレートマッチング時間が長くなることを意味するので、リンクを所定速度以上で動かした場合には、撮像対象物を基準とした座標系での画像センサの位置取得処理と、画像センサ位置に基づいた各関節の状態量演算処理の両方が完了するまでの時間が、遠隔作業ロボットの制御周期を超え、遠隔作業ロボットの適切な制御に失敗する虞があった。

【0008】

そこで、本発明は、遠隔作業ロボットのリンクに設置したセンサの位置取得処理と、センサ位置に基づいた各関節の状態量演算処理の完了までに要する時間を、遠隔作業ロボットの制御周期内に抑制しつつ、遠隔作業ロボットの各関節の最大速度を高めることができる、制御装置、および、制御方法を提供することを目的とする。

【課題を解決するための手段】

【0009】

上記課題を解決するために、本発明に係る制御装置は、２個以上のリンクを連結する関節を備えた遠隔作業ロボットを、何れかのリンクに設置したセンサが認識したセンサ情報に基づいて制御する制御装置であって、関節動作前の第一時刻におけるセンサ情報から検出した第一特徴点の位置と、関節動作後の第二時刻におけるセンサ情報から検出した第二特徴点の位置を比較して、センサ情報上での特徴点の移動量を求めることで、前記センサの姿勢の変化と、前記関節の状態量の変化を推定するセンサ姿勢検出部と、前記センサから前記第一特徴点までの距離と、動作させた関節と前記センサの相対配置と、動作させた関節の動作方向と、に基づいて、前記第一時刻から前記第二時刻までの前記特徴点の前記センサ情報上での移動量が、予め定めた閾値を超えない最大速度を求める関節速度制限部と、該関節速度制限部が求めた前記最大速度の範囲内で前記関節を動作させるロボット制御部と、を有する制御装置とした。

【発明の効果】

【0010】

本発明の制御装置、および、制御方法によれば、遠隔作業ロボットのリンクに設置したセンサの位置取得処理と、センサ位置に基づいた各関節の状態量演算処理の完了までに要する時間を、遠隔作業ロボットの制御周期内に抑制しつつ、遠隔作業ロボットの各関節の最大速度を高めることができる。

【0011】

なお、本発明に関連する更なる特徴、作用、効果は、本明細書の記述、添付図面から明らかになるものである。また、本発明の態様は、要素及び多様な要素の組み合わせ及び以降の詳細な記述と添付される特許請求の範囲の様態により達成され実現される。

【図面の簡単な説明】

【0012】

【図1】実施例１に係る遠隔作業システムの機能ブロック図。

【図2A】遠隔作業ロボットの構成例および画像フレームとの関係を示す図。

【図2B】図２Ａを簡略表示した図。

【図2C】画像フレーム内での特徴点の移動距離を説明する図。

【図3】画像フレーム内における特徴点の例を示す図。

【図4】閾値を決定する手法を説明する図。

【図5】式５の算出方法を例示する図。

【発明を実施するための形態】

【0013】

以下、添付図面を参照して本発明の実施例について説明する。添付図面では、機能的に同じ要素は同じ番号で表示される場合もある。なお、添付図面は本発明の原理に則った具体的な実装例を示しているが、これらは本発明の理解のためのものであり、決して本発明を限定的に解釈するために用いられるものではない。

【0014】

なお、以下の実施例では、当業者が本発明を実施するのに十分詳細にその説明がなされているが、他の実装、形態も可能で、本発明の技術的思想の範囲と精神を逸脱することなく構成、構造の変更や多様な要素の置き換えが可能であることを理解する必要がある。従って、以降の記述をこれに限定して解釈してはならない。また、本明細書及び図面において、実質的に同一の機能又は構成を有する構成要素については、同一の符号を付することにより重複する説明を省略する。

【実施例0015】

まず、図１から図３を用いて、本発明の実施例１に係る遠隔作業システム１００を説明する。

【0016】

図１は、実施例１に係る遠隔作業システム１００の機能ブロック図である。ここに示すように、本実施例の遠隔作業システム１００は、遠隔作業ロボット１と、センサ２と、制御装置３と、を備えている。

【0017】

遠隔作業ロボット１は、高放射線等の過酷環境下で使用される多関節ロボットであり、２つ以上のリンク１１と、各リンクを連結する関節１２を有する。関節１２は、リンク同士の相対角度を変化させる回転関節であっても良いし、リンク同士の距離を変化させる直動関節であっても良い。従って、後述する「関節速度」とは、回転関節の角速度ω、または、直動関節の伸縮速度の何れかを意味する。

【0018】

センサ２は、遠隔作業ロボット１の作業対象物を認識するためのセンサであり、遠隔作業ロボット１の何れかのリンク１１（例えば、最先端のリンク１１ａ）に設置される。センサ２は、具体的には、ステレオカメラや深度カメラ、あるいは、２軸の超音波センサやレーザスキャナである。前者の場合、センサ２からはセンサ情報として画像情報と奥行情報が出力され、制御装置３では、画像情報に基づいて対象物の特徴点を検出する。一方、後者の場合、センサ２からはセンサ情報として奥行情報のみが出力され、制御装置３では、奥行情報に基づいて対象物の特徴点を検出する。

【0019】

制御装置３は、センサ２が出力したセンサ情報（画像情報、奥行情報）に基づいて遠隔作業ロボット１を遠隔制御する装置であり、ロボットリンクモデル３１と、センサ姿勢検出部３２と、ロボット制御部３３と、関節角度検出部３４と、関節速度制限部３５を備えている。なお、この制御装置３は、具体的には、ＣＰＵ等の演算装置、半導体メモリ等の主記憶装置、ハードディスク等の補助記憶装置、および、通信装置などのハードウェアを備えたコンピュータである。そして、演算装置が所定のプログラムを実行することで、センサ姿勢検出部３２等の各機能を実現するが、以下では、このような周知技術を適宜省略しながら、各部の詳細を説明する。

【0020】

ロボットリンクモデル３１は、遠隔作業ロボット１のリンク１１や、関節１２との配置関係、関節１２の種類等、遠隔作業ロボット１の構造を記述するデータである。この記述には、ＵＲＤＦ（Unified Robotics Description Format）などの記述形式が利用可能である。関節角度検出部３４や関節速度制限部３５は、このロボットリンクモデル３１を参照することにより、各関節の動作軸や、関節ＩＤを得ることができる。

【0021】

センサ姿勢検出部３２は、周囲の静止物体を基準とした座標系でのセンサ２の姿勢変化を検出する機能部である。そのため、センサ姿勢検出部３２は、ある時刻（以下「時刻ｔ１」と称する）にセンサ２で検出した第一センサ情報から、周囲の静止物体のエッジ点など、特徴となる特徴点を複数検出し、第一特徴点ｆ１として記憶しておく。その後、センサ姿勢検出部３２は、所定時間後の別の時刻（以下「時刻ｔ２」と称する）にセンサ２で検出した第二特徴点ｆ２と、前述した第一特徴点ｆ１を比較することで、第一特徴点ｆ１の全部または一部が、第二特徴点ｆ２のどの部分に合致するかを検出することで、センサ２の姿勢がどう変化したかを検出する。

【0022】

このようなアルゴリズムは、Visual SLAM(Simultaneous Localization and Mapping)技術として知られる。Visual SLAMの詳細については、例えば、M. Labbe and F. Michaud, “RTAB-Map as an Open-Source Lidar and Visual SLAM Library for Large-Scale and Long-Term Online Operation,” in Journal of Field Robotics, vol. 36, no. 2, pp. 416-446, 2019. (Wiley)が参照可能である。

【0023】

ロボット制御部３３は、遠隔作業ロボット１の各関節に適当な動作指令を送信することで所望の関節１２を順次動作させ、遠隔作業ロボット１を遠隔制御する機能部である。そのため、ロボット制御部３３は、関節角度検出部３４に動作対象の関節１２の関節ＩＤを送信して、その関節１２の関節状態情報を取得する。また、ロボット制御部３３は、関節速度制限部３５に動作対象の関節１２の関節ＩＤと動作方向情報を送信して、その関節１２に設定可能な最大速度情報を取得する。

【0024】

ここで、図２Ａ～図２Ｃ、および、図３を用いて、ロボット制御部３３がある関節１２を動作させた前後の、遠隔作業ロボット１とセンサ２の姿勢変化、および、センサ２が出力した画像フレームＦの変化を具体的に説明する。

【0025】

図２Ａに示す遠隔作業ロボット１は、４つのリンク１１ａ～１１ｄと、３つの関節１２ａ～１２ｃで構成される。また、図２Ａのセンサ２は、深度カメラであり、最先端のリンク１１ａに設置されている。なお、図中の破線は、センサ２の視野角θｃであり、センサ２は、この範囲内で、画像フレームＦを撮像し、被写体までの距離ｄを測定する。

【0026】

図２Ａ上図は、時刻ｔ１におけるロボット姿勢Ｐ１を表しており、図２Ａ下図は、時刻ｔ２におけるロボット姿勢Ｐ２を表している。なお、ロボット姿勢Ｐ２は、アーム根元側の関節１２ｃだけを時計回りに回転させた後の姿勢であるため、センサ２の撮像方向もロボット姿勢Ｐ１に比べて時計回りに変化している。

【0027】

また、図２Ａ上図の特徴点ｆ１は、画像フレームＦ１から画像処理により抽出した静止物体上の特徴点を示し、図２Ａ下図の特徴点ｆ２は、画像フレームＦ２から画像処理により抽出した静止物体上の特徴点を示している。両特徴点は、静止物体の同じ位置から抽出したものであるため、これらの特徴点を基準とすることで、静止物体を基準とする座標系でのセンサ２の位置・姿勢や、関節１２ａ～１２ｃの状態量を特定することができる。

【0028】

ここで、動作対象となる関節１２ｃの三次元空間上での座標を点Ｏ、センサ２（深度カメラ）の時刻ｔ１の座標を点Ｐｃ、時刻ｔ２の座標を点Ｐｃ’、特徴点ｆの座標を点Ｐとする。なお、各座標は行ベクトルで表す。図２Ａの状況下では、関節１２ｃと特徴点ｆの位置は不変であるため、点Ｏ、Ｐは、時刻ｔ１，ｔ２で同一である。また、関節１２ａ、１２ｂを動作させないため、点Ｏから点Ｐｃの距離｜Ｐｃ－Ｏ｜、および、点Ｏから点Ｐｃ’の距離｜Ｐｃ’－Ｏ｜は共にＬである。また、時刻ｔ１および時刻ｔ２の時間差をΔｔとし、関節の角速度をωとすると、時刻ｔ１と時刻ｔ２における関節１２の角度差Δθは、Δθ＝ωΔｔとなる。また、時刻ｔ１における、センサ２から見た奥行方向の単位ベクトルをＶｚとし、時刻ｔ２における、センサ２から見た奥行方向の単位ベクトルをＶｚ’とする。

【0029】

いま、点Ｏを原点としても一般性は失われないため原点として説明する。また、ｘ軸を紙面水平右方向、ｙ軸を紙面奥行直交方向、ｚ軸を紙面垂直上方向とした座標系をロボット座標系と呼ぶ。関節１２ｃの回転軸は、ｙ軸方向とする。ｙ軸中心とした回転角θの回転行列をＲｙ（θ）とすると、
Ｐｃ’＝Ｒｙ（θ）Ｐｃ ・・・（式１）
Ｖｚ’＝Ｒｙ（θ）Ｖｚ ・・・（式２）
の関係である。時刻ｔ１におけるセンサ位置（点Ｐｃ）から特徴点ｆ１までの距離ｄ１は式３で表され、時刻ｔ２におけるセンサ位置（点Ｐｃ’）から特徴点ｆ２までの距離ｄ２は式４で表される。

【0030】

ｄ１＝｜Ｐｃ－Ｐ｜ ・・・（式３）
ｄ２＝｜Ｐｃ’－Ｐ｜ ・・・（式４）
図２Ｂは、図２Ａのロボット姿勢Ｐ１からＰ２への変化を簡略表示したものであり、動作対象の関節１２ｃの位置（点Ｏ）と、センサ２の位置（点Ｐｃ、点Ｐｃ’）、センサ２の姿勢（単位ベクトルＶｚ、Ｖｚ’）のみを模式的に表した図である。ＶｘとＶｙは互いに直交するとともに、時刻ｔ１におけるセンサ２の姿勢を表す単位ベクトルＶｚに直交する単位ベクトルであり、これら三つの単位ベクトルによって、時刻ｔ１におけるセンサ２（深度カメラ）の画像フレームの座標軸を表している。同様に、時刻ｔ２におけるセンサ２（深度カメラ）の画像フレームの座標軸は、Ｖｘ’、Ｖｙ’、Ｖｚ’の三つの単位ベクトルで表される。

【0031】

図２Ｃは、センサ２（深度カメラ）で撮影された、時刻ｔ１における画像フレームＦ１および時刻ｔ２における画像フレームＦ２である。なお、画像フレームの横幅をＷピクセル、高さをＨピクセルとする。また、各画像フレーム上には、単位ベクトルＶｘｃ＝（１，０，０）をｘ軸とし、単位ベクトルＶｙｃ＝（０，１，０）をｙ軸とし、画像フレームの奥行方向の単位ベクトルＶｚｃ＝（０，０，１）をｚ軸とした直交座標系である、画像フレーム座標系を示している。また、画像フレームＦ１における特徴点ｆ１の座標を（ｘ１，ｙ１，ｚ１）、画像フレームＦ２における特徴点ｆ２の座標を（ｘ２，ｙ２，ｚ２）とする。

【0032】

ここで、行列Ｒｃ＝（Ｖｘ；Ｖｙ；Ｖｚ）は、画像フレーム座標系から、ロボット座標系への変換行列を表す。なお、「；」は、１行３列ベクトルを行方向に並べた行列を表すものとする。すると、ｆ１＝（Ｒｃ（Ｐｃ^ｔ－Ｐ^ｔ）／｜Ｐｃ－Ｐ｜）^ｔとなる。ただし、上付きのｔは、転置ベクトルを表す。また、Ｒｃ’＝（Ｖｘ’，Ｖｙ’，Ｖｚ’）＝Ｒｙ（θ）Ｒｃの関係があるため、ｆ２＝（Ｒｙ（θ）Ｒｃ（Ｐｃ’ ^ｔ－Ｐ^ｔ）／｜Ｐｃ’－Ｐ｜）^ｔ＝（Ｒｙ（θ）Ｒｃ（Ｒｙ（θ）Ｐｃ^ｔ－Ｐ^ｔ）／｜Ｒｙ（θ）Ｐｃ－Ｐ｜）^ｔ＝で表される。これにより、画像フレーム内の移動距離｜ｆ１－ｆ２｜が、Δθ＝ωΔｔの関数で表されるため、画像フレーム内の移動距離の閾値Ｔｈを与えた時、｜ｆ１－ｆ２｜＝Ｔｈとなるωを最大の角速度ω_ｍａｘとして算出することができる。

【0033】

次に、図３を用いて、画像フレームＦ間での特徴点ｆの移動を、より具体的に説明する。図３左図は、ロボット姿勢Ｐ１である静止物を撮像した画像フレームＦ１であり、図３右図は、ロボット姿勢Ｐ２で同じ静止物を撮像した画像フレームＦ２である。なお、図示するように、画像フレームＦの右方向をｘ軸の正方向、画像フレームＦの下方向をｙ軸の正方向としているため、各特徴点の画像フレームＦ上での位置は、ｘ、ｙの値で定義することができる。

【0034】

図２Ａ～図２Ｃでは、簡単のために各時刻の特徴点ｆを一点で示したが、実際の画像処理では、エッジ検出などの手法により特徴点ｆを抽出するため、複数の特徴点ｆが検出される。そのため、図３の例では、画像フレームＦ１から４つの特徴点ｆ１ａ～ｆ１ｄが検出され、画像フレームＦ２からも４つの特徴点ｆ２ａ～ｆ２ｄが検出されている。

【0035】

ここで、図２Ａからも分かるように、図３の画像フレームＦ１と画像フレームＦ２の撮像条件の違いは、センサ２の位置と姿勢だけである。そのため、同じ静止物を撮像した両画像フレームからは同一配置の特徴点ｆが検出される。つまり、画像フレームＦ１内の特徴点ｆ１ａ～ｆ１ｄの配置を維持したまま所定量移動させたものが、画像フレームＦ２内の特徴点ｆ２ａ～ｆ２ｄとなっている。従って、特徴点ｆを追跡する画像処理では、画像フレームＦ１の特徴点配置をテンプレートとして、画像フレームＦ２内でテンプレートマッチング処理を実行することにより、画像フレームＦ２の特徴点ｆ２の位置を検出することができる。

【0036】

このような特徴点ｆが検出されたとき、センサ姿勢検出部３２は、センサ２の位置と姿勢の変化を次のように演算する。すなわち、センサ姿勢検出部３２は、まず、特徴点ｆ１ａから特徴点ｆ２ａの移動量Δａ、特徴点ｆ１ｂから特徴点ｆ２ｂの移動量Δｂ、特徴点ｆ１ｃから特徴点ｆ２ｃの移動量Δｃ、特徴点ｆ１ｄから特徴点ｆ２ｄの移動量Δｄをそれぞれ演算した後、移動量Δａ～Δｄを平均した平均移動量Δ_ａｖを検出する。そして、平均移動量Δ_ａｖに基づき、前述のＳＬＡＭ技術を適用することで、センサ姿勢検出部３２は、時刻ｔ１におけるロボット姿勢Ｐ１から時刻ｔ２におけるロボット姿勢Ｐ２への位置と姿勢の変化を検出することができる。そして、センサ姿勢検出部３２は、得られたセンサ姿勢情報を関節角度検出部３４に送る。

【0037】

関節角度検出部３４は、ロボットリンクモデル３１を参照して、ロボット制御部３３から得た関節ＩＤに対応する関節１２の種類、動作軸の情報を取得す得る機能部である。

【0038】

また、関節角度検出部３４は、センサ姿勢検出部３２から得たセンサ姿勢情報に基づいて、ロボット制御部３３の制御対象の関節ＩＤが切り替わった時のセンサ姿勢情報を、当該関節の動作開始時の姿勢（初期姿勢と呼ぶ）として、記憶する機能部である。例えば、図２Ａ上図に例示するように、制御対象が関節１２ｃに切り替わった場合であれば、ロボット姿勢Ｐ１でのセンサ姿勢情報を初期姿勢として記憶する。

【0039】

そして、ロボット制御部３３が、新たに制御対象となった関節１２を動作制御し、該関節が動作すると、リンク１１の移動に伴いセンサ２が移動し、センサ情報が変化するため、センサ姿勢検出部３２が検出するセンサ姿勢も変化する。このとき、関節角度検出部３４は、変化後のセンサ姿勢を、センサ姿勢検出部３２から取得し記憶する。例えば、図２Ａ下図に例示するように、制御対象が関節１２ｃを動作させた場合であれば、ロボット姿勢Ｐ２でのセンサ姿勢情報を最新のセンサ姿勢として記憶する。

【0040】

関節角度検出部３４は、初期姿勢（例えば、図２Ａ上図）と最新のセンサ姿勢（例えば、図２Ａ下図）を比較することで、センサ２の姿勢差分を得る。このとき、動作している関節１２は当該関節（図２Ａの例では、関節１２ｃ）のみであるため、センサ２の姿勢差分は、動かした関節１２の動作量を表す。すなわち、回転関節であれば、姿勢差分が表す回転量であり、直動関節であれば、姿勢差分が表す直動量が、関節の動作量となる。

【0041】

関節角度検出部３４は、上記のようにして得られた各関節の動作量を積算することで、作業ロボットの各関節角度情報を管理する。

【0042】

以上が、センサ情報を用いて関節の動作量を検出する基本的な構成である。

【0043】

しかし、センサ姿勢検出部３２は、第一特徴点ｆ１と第二特徴点ｆ２の比較時に、共通部分が存在しない場合には、センサ２の姿勢検出に失敗し、関節１２の動作量を演算できなかったり、第一特徴点ｆ１と第二特徴点ｆ２の距離が離れすぎていると、比較処理に時間を要し、遠隔作業ロボット１の制御周期に対する時間遅れが顕著になったりしてしまうという問題がある。

【0044】

そこで、関節速度制限部３５は、上記の問題が発生しない範囲内で関節１２を動作させる最大速度を演算してロボット制御部３３に出力する。これにより、ロボット制御部３３は、上記の問題が発生しない速度で関節１２を動作させることができる。

【0045】

＜関節１２の最大速度決定処理＞
本実施例の制御装置３は、図１に示すステップＳ１～Ｓ７の手順により、関節１２の最大速度ｖ_ｍａｘを決定する。なお、最大速度ｖ_ｍａｘの決定処理は、基本的には、作業現場に配置した遠隔作業ロボット１が作業対象物を変更する毎に、或いは、所定時間毎に、実施される処理であるが、遠隔作業ロボット１と作業対象物の相対関係が固定されている場合は、遠隔作業システム１００の工場出荷前、或いは、初期設定時に予め各関節の最大速度ｖ_ｍａｘを決定しておいても良い。

【0046】

まず、ステップＳ１では、関節速度制限部３５は、センサ姿勢検出部３２から、検出したｎ個の特徴点ｆの特徴点情報Ｐｉ（ｉ＝１～ｎ）を得る。なお、特徴点情報Ｐｉは（ｘｉ，ｙｉ，ｄｉ）で定義される３次元の情報であり、ｘｉ，ｙｉは、画像フレームＦにおける特徴点ｆの位置座標（図３参照）、ｄｉは、センサ２から特徴点ｆまでの距離（図２Ａ参照）である。

【0047】

次に、ステップＳ２では、ロボット制御部３３は、動作対象となる関節１２の関節ＩＤおよび動作方向を、関節速度制限部３５に送信する。

【0048】

ステップＳ３では、関節速度制限部３５は、ロボットリンクモデル３１を参照し、当該関節の動作軸および、当該関節からセンサまでの相対位置および姿勢を得る。

【0049】

ステップＳ４では、関節速度制限部３５は、当該関節が、動作軸に沿って速度ｖで動作したときに、ｔ秒後に、各特徴点の、センサ情報の検出範囲内における移動量Δｉを算出する。

【0050】

ステップＳ５では、関節速度制限部３５は、この移動量Δｉの平均移動量Δ_ａｖが閾値Ｔｈ以下となる最大速度ｖ_ｍａｘを逆算する（図２Ａ～図２Ｃ参照）。なお、閾値Ｔｈは、遠隔作業システム１００の工場出荷前、或いは、初期設定時に予め設定されているものとする。

【0051】

ステップＳ６では、関節速度制限部３５は、最大速度ｖ_ｍａｘをロボット制御部３３に送信する。

【0052】

ステップＳ７では、ロボット制御部３３は、当該関節の動作速度を該最大速度ｖ_ｍａｘ以下とし、遠隔作業ロボットを制御する。

【0053】

以上の構成とすることにより、センサ姿勢検出部３２が、センサ姿勢の検出に失敗することなく、ロボットをできるだけ速く動作することが可能となる。

【0054】

以上で説明したように、本発明の制御装置、および、制御方法によれば、遠隔作業ロボットのリンクに設置したセンサの位置取得処理と、センサ位置に基づいた各関節の状態量演算処理の完了までに要する時間を制御周期内に抑制しつつ、遠隔作業ロボットの各関節の最大速度を高めることができる。

【実施例0055】

次に、図４と図５を用いて、本発明の実施例２に係る遠隔作業システム１００を説明する。なお、実施例１との共通点は重複説明を省略する。

【0056】

実施例１の遠隔作業システム１００では、ステップＳ５で利用する閾値Ｔｈが予め設定されていたが、本実施例の遠隔作業システム１００では、閾値Ｔｈを後述のシミュレーションにより決定する。なお、閾値Ｔｈの決定処理は、遠隔作業システム１００の工場出荷前、或いは、初期設定時に予め実施される処理である。

【0057】

図４において、画像フレームＦ１は、遠隔作業ロボット１に設置したセンサ２が時刻ｔ１に実際に撮像した画像フレームであり、ｎ個の画像フレームＦ２（Ｆ２ａ～Ｆ２ｎ）は、センサ２が時刻ｔ２に撮像したもの仮定として扱う仮想的な画像フレームであり、画像フレームＦ１内の特徴点ｆ１を所定の移動量Δ（Δ_ａ～Δ_ｎ）だけ上方に移動させて生成したものである。

【0058】

＜閾値Ｔｈの決定方法＞
次に、図５を用いて、閾値Ｔｈの決定方法を説明する。

【0059】

まず、センサ姿勢検出部３２に、時刻ｔ１の画像フレームＦ１と時刻ｔ２の画像フレームＦ２ａを与え、センサ２の位置・姿勢の特定処理と、関節１２の状態量の特定処理の完了までに要する処理時間ｔｐ_ａを求める。同様に、画像フレームＦ１とフレームＦ２ｂ、画像フレームＦ１とフレームＦ２ｃ、・・・、画像フレームＦ１とフレームＦ２ｎを順次与え、処理時間ｔｐ_ｂ～ｔｐ_ｎをそれぞれ求める。

【0060】

このようにして求めた処理時間ｔｐ_ａ～ｔｐ_ｎは、特徴点ｆの移動量Δ_ａ～Δ_ｎにおおよそ比例した時間となるため（図５参照）、処理時間ｔｐと移動量Δの関係は、式５の近似式で表される。

【0061】

ｔｐ＝ＫΔ＋ｔ_０ ・・・（式５）
また、ロボット制御部３３の制御周期をＴ_０とすると、閾値Ｔｈは、式１のＫとｔ_０を用いて、式６により算出することができる。

【0062】

Ｔｈ＝（Ｔ_０－ｔ_０）／Ｋ ・・・（式６）
なお、センサ姿勢検出部３２の演算処理性能が十分であれば、図５に例示するように、Ｔ_０＞ｔ_０の関係が成立するが、センサ姿勢検出部３２の演算処理性能が不足し、ｔ_０がＴ_０より大きくなると、式２により求まるＴｈは負の値となり、適切なＴｈが得られない。従って、この場合は、センサ姿勢検出部３２の処理性能を高くするか、ロボット制御部３３の制御をｎ回分、センサ姿勢検出部３２の処理を待つ制御方式とする必要がある。この場合は、ｎ×Ｔ_０≧ｔ_０となる最小の正数ｎから求められる。

【0063】

以上のような構成とすることにより、実施例１の最大速度ｖ_ｍａｘを求める処理で利用する適切な閾値Ｔｈを定めることが可能となる。

【0064】

本技術分野の通常の知識を有する者には、本発明のその他の実装がここに開示された本発明の明細書及び実施例の考察から明らかになる。記述された実施例の多様な態様及び／又はコンポーネントは、データを管理する機能を有するコンピュータ化ストレージシステムに於いて、単独又は如何なる組み合わせでも使用することが出来る。明細書と具体例は典型的なものに過ぎず、本発明の範囲と精神は後続する請求範囲で示される。