特許 6948164 作業用ロボットのアーム姿勢制御システムおよび方法

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

(12)【公報種別】特許公報(B2)

(11)【特許番号】6948164

(24)【登録日】2021年9月22日

(45)【発行日】2021年10月13日

(54)【発明の名称】作業用ロボットのアーム姿勢制御システムおよび方法

(51)【国際特許分類】

   B25J 13/00 20060101AFI20210930BHJP

   B25J 13/08 20060101ALI20210930BHJP

   B25J 13/06 20060101ALI20210930BHJP

【ＦＩ】

   B25J13/00 Z

   B25J13/08 A

   B25J13/06

【請求項の数】13

【全頁数】15

(21)【出願番号】特願2017-115092(P2017-115092)

(22)【出願日】2017年6月12日

(65)【公開番号】特開2019-918(P2019-918A)

(43)【公開日】2019年1月10日

【審査請求日】2020年1月16日

(73)【特許権者】

【識別番号】507250427

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

(74)【代理人】

【識別番号】110001689

【氏名又は名称】青稜特許業務法人

(72)【発明者】

【氏名】丸山 貴志子

(72)【発明者】

【氏名】木村 宣隆

【審査官】 松浦 陽

(56)【参考文献】

【文献】 特開２０１５−２２４８７５（ＪＰ，Ａ）

【文献】 米国特許出願公開第２０１５／０１６８１３６（ＵＳ，Ａ１）

【文献】 特開２００９−２８７２９８（ＪＰ，Ａ）

【文献】 特開２０００−０８４８７６（ＪＰ，Ａ）

【文献】 特開２０１０−１３７３１２（ＪＰ，Ａ）

【文献】 特開平０１−２６４７８５（ＪＰ，Ａ）

【文献】 国際公開第２０１２／０８１１９７（ＷＯ，Ａ１）

【文献】 特開２０１４−１２１７５９（ＪＰ，Ａ）

【文献】 特開２０１６−２２１６０２（ＪＰ，Ａ）

【文献】 特開２０１５−１６０２５３（ＪＰ，Ａ）

【文献】 特開２０１２−０１１４９８（ＪＰ，Ａ）

【文献】 特開昭６３−２８８６９６（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ １／００ − ２１／０２

(57)【特許請求の範囲】

【請求項1】

可動部を有するアームを備える作業用ロボットのアーム姿勢制御システムであって、
前記アームに付加されたマーカを撮影したカメラ画像を入力とし、
前記カメラ画像から前記マーカを抽出し、前記マーカの３次元座標を生成する画像解析装置と、
前記マーカの３次元座標と前記アームの構造モデルに基づいて、前記アームの姿勢を示すアーム姿勢データを生成するアーム姿勢推定装置と、
前記アーム姿勢データに基づいて、前記アームの姿勢を制御するアーム姿勢制御装置を備え、
前記画像解析装置において、所定個数の前記マーカを抽出できなかった場合、前記画像解析装置において、所定の前記マーカを抽出できなかった場合、および、前記アーム姿勢推定装置において、前記マーカの３次元座標と前記アームの構造モデルに基づくアームの姿勢と過去のアーム姿勢データから推定したアームの姿勢が所定以上異なることにより、前記アーム姿勢データが生成できなかった場合、の少なくとも一つを示す所定のエラーを検出した場合に、
前記所定のエラーの検出以前のアームの姿勢、もしくは基本姿勢に復帰するか、
あるいは、
所定のエラーを検出した状況を記憶し、その後の前記アームの姿勢の制御では、その状況を避けるような回避制御を行う、
作業用ロボットのアーム姿勢制御システム。

【請求項2】

前記アーム姿勢制御装置は、前記アームの姿勢を制御するために液圧を制御し、
前記所定のエラーの検出以前のアームの姿勢に復帰する際には、
前記所定のエラーの前後の液圧の差分を検出し、当該差分にもとづいて前記アームを制御する、
請求項１記載の作業用ロボットのアーム姿勢制御システム。

【請求項3】

前記アーム姿勢制御装置は、前記アームの姿勢を制御するために液圧を制御し、
前記基本姿勢に復帰する際には、
前記アームが有する複数の駆動箇所のすべてに関わる液圧を最小にすることで制御する、
請求項１記載の作業用ロボットのアーム姿勢制御システム。

【請求項4】

前記アーム姿勢制御装置は、前記アームの姿勢を制御するために液圧を制御し、
前記基本姿勢に復帰する際には、
前記アームが有する複数の駆動箇所のすべてに関わる液圧を最大にすることで制御する、
請求項１記載の作業用ロボットのアーム姿勢制御システム。

【請求項5】

前記アーム姿勢制御装置は、前記アームの姿勢を制御するために液圧を制御し、
前記基本姿勢に復帰する際には、
前記アームが有する複数の駆動箇所のうち少なくとも一つに関わる液圧を最小にし、前記アームが有する複数の駆動箇所のうち少なくとも一つに関わる液圧を最大にすることで制御する、
請求項１記載の作業用ロボットのアーム姿勢制御システム。

【請求項6】

前記アーム姿勢制御装置は、現在のアーム姿勢データと最終目標姿勢から動作計画を生成し、現在のアームの姿勢が前記最終目標姿勢に到達していない場合には、前記動作計画に基づいて定められたアームの軌道を所定量ずつ区分したアーム微小移動目標量を求め、当該アーム微小移動目標量に基づいて微小移動を継続し、
前記回避制御では、
前記所定のエラーの検出直前の、アーム微小移動目標量を用いないように制御を行う、
請求項１記載の作業用ロボットのアーム姿勢制御システム。

【請求項7】

前記回避制御では、
前記所定のエラーの検出直前の、前記アーム姿勢データに対応する姿勢を避けるように制御を行う、
請求項１記載の作業用ロボットのアーム姿勢制御システム。

【請求項8】

前記回避制御では、
前記所定のエラーの検出直前および直後の少なくとも一つに対応した前記マーカの３次元座標をマッピングし、回避すべきエリアを規定し、当該エリアを避けるように制御を行う、
請求項１記載の作業用ロボットのアーム姿勢制御システム。

【請求項9】

可動部を有するアームを備える作業用ロボットのアーム姿勢制御方法であって、
前記アームに付加されたマーカを撮影したカメラ画像を入力する入力ステップと、
前記カメラ画像から前記マーカを抽出し、前記マーカの３次元座標を生成する画像解析ステップと、
前記マーカの３次元座標と前記アームの構造モデルに基づいて、前記アームの姿勢を示すアーム姿勢データを生成するアーム姿勢推定ステップと、
前記アーム姿勢データに基づいて、アーム微小移動目標量を求め、前記アーム微小移動目標量に基づいて、前記アームの姿勢を制御するアーム姿勢制御ステップを実行し、
前記画像解析ステップにおいて、所定個数の前記マーカを抽出できなかった場合、前記画像解析ステップにおいて、所定の前記マーカを抽出できなかった場合、および、前記アーム姿勢推定ステップにおいて、前記マーカの３次元座標と前記アームの構造モデルに基づくアームの姿勢と過去のアーム姿勢データから推定したアームの姿勢が所定以上異なることにより前記アームの姿勢を示すアーム姿勢データを生成できなかった場合、の少なくとも一つを示す所定のエラーを検出した場合に、
前記アーム姿勢データに基づいて制御されたアームの姿勢、もしくは基本姿勢に復帰する復帰制御を行うか、
あるいは、
前記所定のエラーを検出した状況を記憶し、その後の前記アームの姿勢の制御では、その状況を避けるような回避制御を行う、
作業用ロボットのアーム姿勢制御方法。

【請求項10】

前記アームの姿勢を制御するために液圧を制御し、
前記アーム姿勢データに基づいて制御されたアームの姿勢に復帰する際には、
前記アーム姿勢データを生成できなくなったタイミングの前後の液圧の差分を検出し、当該差分にもとづいて前記アームを制御する、
請求項９記載の作業用ロボットのアーム姿勢制御方法。

【請求項11】

前記アームの姿勢を制御するために液圧を制御し、
前記基本姿勢に復帰する際には、
前記アームが有する複数の駆動箇所に対する液圧を所定の設定値にすることで制御する、
請求項９記載の作業用ロボットのアーム姿勢制御方法。

【請求項12】

前記アームの姿勢を制御するために液圧を制御し、
前記基本姿勢に復帰する際には、
前記アームが有する複数の駆動箇所のすべてに関わる液圧を最小にすることで制御する、
請求項１１記載の作業用ロボットのアーム姿勢制御方法。

【請求項13】

前記アームの姿勢を制御するために液圧を制御し、
前記基本姿勢に復帰する際には、
前記アームが有する複数の駆動箇所のすべてに関わる液圧を最大にすることで制御する、
請求項１１記載の作業用ロボットのアーム姿勢制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、ロボットの姿勢を制御する技術に関し、特に作業用ロボットのアーム姿勢の制御技術に関する。

【背景技術】

【0002】

従来の作業用ロボットの例として、特許文献１には、取り扱う重量が大きい液圧駆動アクチュエータおよび、それを組み込んだ筋肉ロボットが提案されている。

【0003】

一般的に、作業用ロボットの操作では、オペレータがロボットを直接目視し、あるいは、カメラなどで撮影した遠隔地のロボットの動きを見ながら、ロボットの操作を行うことが考えられる。また、ロボットの実際の動作を決定する際には、情報処理装置の支援を受けることもできる。

【0004】

特許文献２には、ロボットの動作計画法として、初期姿勢と目標姿勢を補間する姿勢を無数にサンプリングし、所定のコスト関数に基づいてコストが最小になるように軌道を決定する手法が開示される。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特開２０１６−２０３３５９号公報

【特許文献2】特開２０１５−１６０２５３号公報

【発明の概要】

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

【0006】

原子炉の廃炉作業等では、周囲の構造物が未知であり、かつ、高放射線環境という人の立ち入りが困難な条件下での作業が想定される。このため、原則として、遠隔操作のロボットによる作業が想定される。

【0007】

ロボットが作業する際には、作業環境において、ロボット自身の損傷や周囲の施設の破損を避けるために、周囲の構造物との干渉を制御する必要がある。このため、ロボットの位置や姿勢を正確にコントロールすることが要求される。

【0008】

しかし、一般の産業用ロボットに搭載されているような、関節の角度を直接計測するエンコーダ等の電子部品を用いたセンサは、高い放射線の環境下では誤動作を起こす可能性がある。このため、ロボットの姿勢をセンサにより直接把握することができない場合がある。

【0009】

また、ビデオカメラで画像を取得し、画像に基づいてロボットの姿勢を把握しようとした場合、高い放射線はカメラの画像にもノイズを発生させる場合がある。また、高温高湿度の環境下では、水蒸気等の影響により、カメラの画像が不鮮明になる場合もある。さらに、ビデオカメラの配置や照明器具の設置にも制約がある。ビデオカメラの画像の明瞭性を十分に確保できない場合、画像を見てロボットを操作するオペレータの作業効率も低下するおそれがある。

【0010】

そこで本発明の目的は、高い放射線の環境下で使用可能な、遠隔操作ロボットのオペレータによる操作性を向上することを目的とする。

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

【0011】

本発明の一側面は、可動部を有するアームを備える作業用ロボットのアーム姿勢制御システムである。当該システムは、アームに付加されたマーカを撮影したカメラ画像を入力として処理を行う。システムの構成要素としては、カメラ画像からマーカを抽出し、マーカの３次元座標を生成する画像解析装置と、マーカの３次元座標とアームの構造モデルに基づいて、アームの姿勢を示すアーム姿勢データを生成するアーム姿勢推定装置と、アーム姿勢データに基づいて、アームの姿勢を制御するアーム姿勢制御装置を備える。また、アーム姿勢データの生成において、所定のエラーを検出した場合に、所定のエラーの検出以前のアームの姿勢、もしくは基本姿勢に復帰するか、あるいは、所定のエラーを検出した状況を記憶し、その後のアームの姿勢の制御では、その状況を避けるような回避制御を行う。

【0012】

本発明の他の一側面は、可動部を有するアームを備える作業用ロボットのアーム姿勢制御方法である。この方法は、アームに付加されたマーカを撮影したカメラ画像を入力する入力ステップと、カメラ画像からマーカを抽出し、マーカの３次元座標を生成する画像解析ステップと、マーカの３次元座標とアームの構造モデルに基づいて、アームの姿勢を示すアーム姿勢データを生成するアーム姿勢推定ステップと、アーム姿勢データに基づいて、アーム微小移動目標量を求め、アーム微小移動目標量に基づいて、アームの姿勢を制御するアーム姿勢制御ステップを実行する。また、アームの姿勢を示すアーム姿勢データを生成できなかった場合に、アーム姿勢データに基づいて制御されたアームの姿勢、もしくは基本姿勢に復帰する復帰制御を行うか、あるいは、アーム姿勢データを生成できなかった状況を記憶し、その後のアームの姿勢の制御では、その状況を避けるような回避制御を行う。

【発明の効果】

【0013】

遠隔操作ロボットの操作性を向上することができる。

【図面の簡単な説明】

【0014】

【図1】実施例のロボットのアーム姿勢制御システムの、ハードウェアの全体構成を示す斜視図。

【図2】実施例のロボットのアーム姿勢制御システムの、全体の処理の流れを示すフロー図。

【図3】ロボットのアーム姿勢制御システムの機能ブロックを示すブロック図。

【図4】画像解析装置の処理フローを示す流れ図。

【図5】アーム姿勢推定装置における、アーム姿勢推定処理の処理フローを示す流れ図。

【図6】アーム姿勢制御装置における、アーム姿勢制御処理の処理フローを示す流れ図。

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

【0015】

以下で説明される実施例の一つの例では、アームを有するロボット（作業用ロボット）を含むロボットシステムであって、作業環境あるいはロボット自体に設置された非接触センサを用いる。非接触センサとしては、例えばステレオカメラがある。アームには複数のマーカが配置されており、ロボットシステムは、マーカを非接触センサで検知することで、アームの姿勢を把握する。この構成によれば、放射線耐性に優れる構成によって、ロボットの姿勢を把握することができる。

【0016】

そして、非接触センサでアームの検知ができなくなった場合には、その前の姿勢あるいは基本姿勢に復帰する。基本姿勢とは、例えば前述の特許文献１に開示される作業用ロボットでは、液圧を最大または最小とした場合の姿勢である。基本姿勢は、液圧などの制御パラメータを所定条件あるいは所定値にした場合、一意に定まる姿勢と定義できる。当該構成によれば、非接触センサでアームの検知ができず、姿勢が認識できない場合であっても、認識可能な姿勢に復帰することができる。

【0017】

また、アームの検知ができなくなった位置を記憶し、その後の動作では、その位置を避けてアームを駆動する。非接触センサでアームの検知ができなくなった場合には、例えば障害物などにより、カメラからマーカが見えない場合が想定される。そこで、そのような位置をあらかじめ避けるようにして、アームの姿勢を制御することにより、継続的にアームの姿勢を把握することを可能とする。

【0018】

（１.システムの全体ハードウェア構成）
図１は、本発明の実施例である、ロボットのアーム姿勢制御システムの、ハードウェアの全体構成を示す斜視図である。

【0019】

アーム１００１を有するロボット１は、例えば移動機構１００２で移動が可能である。アーム１００１は、一つあるいは複数の関節１００３を備え、関節１００３は角度を変化させたり、回転させたりすることが可能である。アーム１００１は、遠隔地にいるオペレータ等の指示により姿勢を変化させることができ、作業対象物７に対して保持部１００４等により作業を行うことが可能である。アーム１００１の機械的な構造については、例えば特許文献１に開示がある。特許文献１に開示される作業用ロボットは、液圧で動作する構造となっており、特に水圧は、ロボットからの油漏れなどによる、作業環境への影響が小さいことが期待できる。

【0020】

アーム１００１には一つまたは複数のマーカ３が配置されている。本実施例では、マーカは視認性の良い構成を持つ。マーカ３は例えば、所定の大きさの白色の円盤であり、アーム１００１の各部に取り付けられる。マーカ３は発光ダイオード等で発光させてもよい。また、部位ごとにマーカの大きさや色を変えてもよい。マーカ３の構成としては、ロボットの作業環境で、マーカ３が十分に検出できるように、背景に対してコントラストが明確な色や形を選択することが望ましい。

【0021】

アーム１００１にとりつけられたマーカ３は、ステレオカメラ２により撮影される。ステレオカメラ２は、例えば２台のビデオカメラである。ステレオカメラ２で撮影された画像は、画像解析装置６に送信される。画像解析装置６の出力は、アーム姿勢推定装置５に入力され、アーム姿勢推定装置５の出力は、アーム姿勢制御装置４に入力される。アーム姿勢制御装置４からの出力は、アーム１００１に入力され、アーム１００１の姿勢を制御する。

【0022】

例えば特許文献１の技術では、アームの姿勢を制御するために液圧を用いている。この方式を採用する場合には、アーム姿勢制御装置４は液体の圧力を制御するためのポンプを含むものとし、ポンプから送出される液体自体をアーム姿勢制御装置４からの出力と考えてよい。

【0023】

（２.システムの全体処理フロー）
図２は、本実施例のロボットのアーム姿勢制御システムの全体の処理の流れを示す図である。

【0024】

処理Ｓ１０１では、ステレオカメラ２でロボット１のアーム１００１およびマーカ３を撮影し、ステレオカメラ画像を取得する。

【0025】

処理Ｓ１０２では、画像解析装置６により、画像解析処理が行われる。後に図４で詳しく説明する。

【0026】

処理Ｓ１０３では、アーム姿勢推定装置５により、アーム姿勢推定処理が行われる。当該処理により、オペレータあるいはシステムは、ロボット１やアーム１００１の姿勢を把握することができる。後に図５で詳しく説明する。

【0027】

処理Ｓ１０４では、アーム姿勢制御装置４により、アーム姿勢制御処理が行われる。当該処理は、オペレータあるいはシステムが、ロボット１やアーム１００１に次の姿勢を指示する処理である。後に図６で詳しく説明する。

【0028】

処理Ｓ１０５では、アーム姿勢制御装置４から、ロボット１に対して、アーム動作指示が出力される。

【0029】

（３．システムの機能ブロック構成）
図３は、本発明の一実施例である、ロボットのアーム姿勢制御システムの機能ブロックを示すブロック図である。アーム姿勢制御装置４、アーム姿勢推定装置５、画像解析装置６は、信号ケーブルあるいは無線リンクで接続されており、信号の送信あるいは受信が可能である。

【0030】

本実施例では、アーム姿勢制御装置４、アーム姿勢推定装置５、画像解析装置６は、プロセッサ、メモリ、入力インタフェースおよび出力インタフェースを備える、通常のサーバで実現することにした。各装置の計算や制御等の機能は、メモリに格納されたプログラムがプロセッサによって実行されることで、定められた処理を他のハードウェアと協働して実現される。計算機などが実行するプログラム、その機能、あるいはその機能を実現する手段を、「機能」、「手段」、「部」、「ユニット」、「モジュール」等と呼ぶ場合がある。以上の構成は、単体のコンピュータで構成してもよいし、あるいは、プロセッサ、メモリ、入力インタフェースおよび出力インタフェースの任意の部分が、相互に接続された他のコンピュータで構成されてもよい。

【0031】

図１や図３では、アーム姿勢制御装置４、アーム姿勢推定装置５、画像解析装置６は別個の装置として説明しているが、一つの装置としてもよい。すなわち、３台のサーバで構成してもよいし、複数の機能を持つ１台のサーバで構成してもよい。

【0032】

図３に示した構成のうち、ロボット１およびロボット１を撮影するステレオカメラ２は、高放射線の作業環境に配置される場合がある。この場合には、通常のサーバの構成部品、特にＳＲＡＭ（Static Random Access Memory）などの電子デバイス、は耐放射線性能に乏しいので、アーム姿勢制御装置４、アーム姿勢推定装置５、画像解析装置６を構成するサーバは、作業環境から遠隔地に設置することが望ましい。サーバとステレオカメラ２との間には、画像データを送信する信号ケーブルを配置して距離を確保することができる。

【0033】

また、サーバとロボット１との間には、ロボット１にアーム動作指示信号を送る信号線を配置して距離をとることができる。あるいは、前述のように、アーム１００１が水や油など液体の圧力で動作する場合には、液体を送るためのパイプを設置することもできる。この場合には、アーム動作制御部７０３からの信号によって、図示しないポンプ等を制御し、ポンプ等の働きにより、アーム１００１の各部に供給される液体の圧力を制御するものとする。

【0034】

（３-１．ステレオカメラ）
ステレオカメラ２は、一般には電子部品の素子スケールが小さいほど耐放射線性能が低下するので、マーカ３が認識できる程度の解像度があればよく、高精細画像は必要ではない。ステレオカメラ２は、ステレオカメラ画像信号Ｖを生成し、画像解析装置６に送信する。また、ステレオカメラ２は、画像解析管理部６０１からの指示信号Ｃによって、撮影の開始、終了、あるいは撮影角度の変更を行うものとする。

【0035】

ステレオカメラの個数は１または複数である。アーム１００１の画像に死角がないことが理想的なので、３個以上設置しても良い。ステレオカメラの相対的な位置関係や光軸方向は、設置時に設定する、設定後実測する、あるいはカメラ画像から推定するなどの方法により、データ化しておくものとする。

【0036】

（３-２．画像解析装置（概要））
図４に画像解析装置６の処理フローを示し、図３を参照しつつ処理内容を説明する。本実施例の画像解析装置６は、画像解析管理部６０１が全体の動作を制御するものとする。画像解析装置６の画像解析管理部６０１は、ステレオカメラ２からのステレオカメラ画像信号Ｖを受信すると、ステレオカメラ画像信号Ｖをマーカ検出部６０２に入力する（Ｓ２０１）。ステレオカメラ画像は、２以上のカメラからの画像を含むものとする。

【0037】

マーカ検出部６０２は、ステレオカメラ画像信号Ｖから画像処理によりマーカ部分を検出し、ステレオカメラの原理に従って、各マーカの３次元座標を検出する。さらに、必要によりマーカ３次元予測座標３ＤＥを用いてマーカ座標を補完する（Ｓ２０２）。処理Ｓ２０２の結果得られた、マーカ３次元座標３Ｄは、画像解析管理部６０１に送られる。座標変換部６０３、マーカ３次元予測座標３ＤＥ、およびマーカ座標の補完については、後に説明する。

【0038】

マーカを検出する画像処理では、例えばマーカ３が白色の円形である場合には、高コントラストの円形の部分を検出する。マーカの色や反射率を変更し、色の検出や反射光の検出によって検出してもよい。そして、２次元画像中のマーカ３の座標を示すマーカ２次元座標を生成する。

【0039】

画像解析装置６はデータとして、センサ配置情報６０４を持つ。センサ配置情報６０４は、センサ例えばカメラの位置座標および画角（カメラの光軸の方向および視野角）である。マーカ検出部６０２は、センサ配置情報６０４を用いて、カメラからみた２次元座標を、３次元座標に変換し、マーカ３次元座標３Ｄとして、画像解析管理部６０１に出力する。この際、前述のように必要に応じてマーカ３次元座標の補完を行う。

【0040】

複数の画像から３次元を復元する方法としては、ＳｆＭ（Structure from Motion）として知られている手法がある。３次元座標の原点や座標系は任意でよい。画像解析管理部６０１は、マーカ３次元座標３Ｄをアーム姿勢推定装置５に送信する（Ｓ２０３）。

【0041】

（３-３．アーム姿勢推定装置）
図５にアーム姿勢推定装置５における、アーム姿勢推定処理の処理フローを示し、図３を参照しつつ処理内容を説明する。図５に示すように、アーム姿勢推定装置５の姿勢更新部５０１は、画像解析装置６の画像解析管理部６０１からマーカ３次元座標３Ｄを受信する。ステレオカメラ画像信号Ｖは、時系列の複数のフレーム画像を含み、各フレームからマーカ３次元座標３Ｄを生成することにより、マーカ３の動きを追跡することができる。

【0042】

アーム姿勢推定装置５の姿勢更新部５０１は、アーム姿勢を推定し、アーム姿勢データＡＰとして更新する処理を行う。アーム姿勢推定装置５は、アーム姿勢データＡＰを時系列にアーム姿勢履歴データ５０３として蓄積する。アーム姿勢データＡＰの推定の処理の内容については後述するが、本実施例では、検出したマーカに基づくマーカ３次元座標３Ｄと、アーム姿勢履歴データ５０３（過去のアーム姿勢データＡＰ）から予測したアーム姿勢予測データＡＰＥの両方を利用する。

【0043】

図３に示すように、アーム姿勢履歴データ５０３は、姿勢予測部５０４に入力される。姿勢予測部５０４では、アームの運動モデル５０５を用いて、アーム姿勢履歴データ５０３からアーム姿勢予測データＡＰＥを生成する（Ｓ３０１）。アームの運動モデル５０５は例えば、アームの各関節が所定の等角速度で動作するというような制約条件に従うモデルである。このような予測は、例えばカルマンフィルターを用いて行うことができる。

【0044】

姿勢更新部５０１には、画像解析管理部６０１から、マーカ３次元座標３Ｄが入力される（Ｓ３０２）。姿勢更新部５０１は、アームモデル５０２を用いて、マーカ３次元座標３Ｄからアームの姿勢を算出する。ここで、アームモデル５０２は、アーム１００１の関節と関節の接続関係、関節と関節の間隔、関節の可動範囲、各関節軸を基準としたマーカの位置の情報等を含む。アームが液体の圧力で駆動される場合には、当該姿勢に対応した液体の圧力としてもよい。

【0045】

姿勢更新部５０１は、アームモデル５０２とマーカ３次元座標３Ｄから、アームの姿勢を一意に特定するためのパラメータを算出する。このパラメータは、例えばアームの各関節の角度、あるいはその角度を取るための液体の圧力である。アーム１００１が伸縮機能を持つ場合には、伸縮長を含んでもよい。本実施例では、アーム１００１を液体の圧力で駆動することにし、アームの姿勢およびアームを駆動するための制御信号は、アームの各部に供給される液体の圧力で規定することにする。

【0046】

姿勢更新部５０１では、マーカ３次元座標３Ｄとアームモデル５０２から算出したアームの姿勢と、姿勢予測部５０４で予測したアーム姿勢予測データＡＰＥとから、アーム姿勢を推定し、アーム姿勢データＡＰとして、アーム姿勢推定管理部５０６に入力する。

【0047】

アーム姿勢の具体的な推定手法としては、例えば、マーカ３次元座標３Ｄから算出したアームの姿勢から尤度関数を評価し、カルマンフィルターのパラメータを補正して、アーム姿勢データＡＰを推定し更新する（Ｓ３０３）。アーム姿勢データＡＰはアーム姿勢推定管理部５０６から、アーム姿勢制御装置４に送信される。

【0048】

なお、アーム姿勢履歴データ５０３が十分に蓄積されていない間は、マーカ３次元座標３Ｄから算出したアームの姿勢をそのままアーム姿勢データＡＰとしてよい。

【0049】

一方、本実施例では図３に示すように、姿勢予測部５０４で生成されたアーム姿勢予測データＡＰＥは、画像解析装置６の画像解析管理部６０１にフィードバックされている。アーム姿勢予測データＡＰＥを用いると、マーカ検出もれの補完を行うことができる。

【0050】

（３-４．画像解析装置（マーカ座標の補完））
図４に戻ってマーカ検出もれの補完処理を説明する。アーム姿勢予測データＡＰＥは座標変換部６０３に入力され（Ｓ２０４）、アームモデル６０５（アームモデル５０２と同一内容でよい）に基づいて、マーカ３次元予測座標３ＤＥに変換される（Ｓ２０５）。マーカ３次元予測座標３ＤＥは、画像解析管理部６０１を経由してマーカ検出部６０２に送られる。

【0051】

マーカ検出部６０２では、ステレオカメラ２の画像によって検出された信号に基づくマーカ３次元座標を、マーカ３次元予測座標３ＤＥによって補完し、マーカ３次元座標３Ｄを生成する。すなわち、マーカ３次元予測座標３ＤＥは、姿勢予測部５０４によって予測されたアームの姿勢に基づく、あるべきマーカの座標であり、障害物や水蒸気によりマーカ３が隠れた場合でも、データの欠落に起因するマーカの検出漏れを補完することができる。例えば、ステレオカメラ２の画像によって２つのマーカが検出され、その間のマーカが検出されなかった場合、マーカ３次元予測座標３ＤＥはその間のマーカの存在を示唆することができる。ただし、例えばアーム先端部のマーカの補完は、アーム角度を知り得ない状況では難しい。

【0052】

以上説明したように、図４に示すごとく、マーカ検出部６０２では、マーカ３次元予測座標３ＤＥを用いて、ステレオカメラ２の画像から得られたマーカの情報を補完する（Ｓ２０２）。そして、マーカ３次元座標３Ｄを生成し、画像解析管理部６０１に出力する（Ｓ２０３）。本実施例では、以上のように姿勢予測部５０４でのアーム姿勢予測データＡＰＥを、画像解析装置６でのマーカ検出の補完に用いているが、補完処理は場合によっては省略してもよい。

【0053】

ロボット１の作業環境では、以上の補完を行った場合であっても、周辺の障害物や水蒸気などにより、十分にロボット１の姿勢の一部または全部を認識できない場合がある。例えば、画像解析管理部６０１は、所定の個数以上のマーカの座標を認識できなかった場合や、アーム先端のマーカを見失った場合には、マーカ３次元座標３Ｄの代わりに、もしくはこれに付加して、ロボットの姿勢を認識できないことを示すエラー信号をアーム姿勢推定装置５に送信する。エラー信号を受信したアーム姿勢推定装置５は、アーム姿勢データＡＰの代わりに、もしくはこれに付加して、エラー信号をアーム姿勢履歴データ５０３に格納し、また、アーム姿勢制御装置４に送信する。このとき、エラー信号には、直前のマーカ３次元座標３Ｄ、あるいは直前のアーム姿勢データＡＰをさらに付加してもよい。

【0054】

また、エラー信号は、上記のように画像解析管理部６０１が生成する代わりに、姿勢更新部５０１において、アーム姿勢が推定できなかった場合に生成することにしてもよい。アーム姿勢が推定できない場合とは、例えばマーカ３次元座標３Ｄとアームモデル５０２から算出したアームの姿勢と、姿勢予測部５０４で予測したアーム姿勢予測データＡＰＥが所定以上異なる場合である。

【0055】

（３-５．アーム姿勢制御装置）
図６にアーム姿勢制御装置４における、アーム姿勢制御処理の処理フローを示し、図３を参照しつつ処理内容を説明する。アーム姿勢制御装置４のアーム動作計画部７０１は、アーム姿勢データＡＰをアーム姿勢推定装置５から受け取る。アーム動作計画部７０１は、最終目標設定部７０２にアーム姿勢データＡＰを送る。

【0056】

アームの姿勢制御の初期設定において、最終目標設定部７０２は、アーム１００１の最終目標となる最終目標姿勢ＦＰを設定する（Ｓ４０１）。最終目標姿勢ＦＰは、例えばアーム姿勢データＡＰからアーム１００１の姿勢を画像で再現して画像表示装置（図示せず）に表示し、オペレータが確認して、例えば保持部１００４を移動させたい位置を画像上で指定するようにしてもよい。あるいは他の公知の方法でもよい。またこのとき、ステレオカメラ画像信号Ｖによる画像をあわせて表示してもよい。なお、説明上、最終目標姿勢ＦＰを設定する時点ではアーム１００１は見えているものとする。

【0057】

システムが扱うデータとして設定される最終目標姿勢ＦＰは、例えばその姿勢をとるためのアーム１００１の各関節の角度や液圧で定義される。本実施例では最終目標姿勢ＦＰは、アームの姿勢およびアームを駆動するための制御信号と同じく、アーム１００１の各部に供給される液体の圧力で規定することにする。このときオペレータから指示されたアームの姿勢は、その姿勢をとるためにアームの各部に供給される液体の圧力に変換すればよい。設定された最終目標姿勢ＦＰは、アーム動作計画部７０１に送信される。

【0058】

アーム動作計画部７０１では、現在のアーム姿勢データＡＰと最終目標姿勢ＦＰから、動作計画を生成する（Ｓ４０２）。ロボットの制御においては、環境の変化や、目標姿勢の変更に対応するために、環境や初期姿勢・目標姿勢にあわせて動的に軌道を生成する動作計画法という技術が知られている。動作計画とは、初期姿勢と目標姿勢と周囲環境の情報を入力に、何らかの基準に基づいてロボットの動作の軌道を決定する技術である。動作計画問題は産業用腕型ロボットにおいては、どのように腕を動かせば指定の姿勢になるのかを探す問題である。このような動作計画法の一例は、特許文献２にも開示がある。

【0059】

アーム動作計画部７０１では、最新のアーム姿勢データＡＰを入力する（Ｓ４０３）。そして、アーム１００１の姿勢が把握できているかを判定する（Ｓ４０４）。この判定は、画像解析管理部６０１、あるいは、姿勢更新部５１０で生成されたアーム姿勢データＡＰの内容、あるいはエラー信号（上述）の有無に基づいて可能である。最終目標姿勢ＦＰを設定する時点ではアームが見えていることが保証されている場合には、最初の微小移動前において、このステップは省略してよい。

【0060】

アーム動作計画部７０１は、最新のアーム姿勢データＡＰに基づいて、アーム１００１の姿勢が最終目標姿勢ＦＰであるかどうかを判定する（Ｓ４０５）。アームの姿勢が把握できており、現在のアームの姿勢が最終目標姿勢である場合には、アーム動作制御を終了する。

【0061】

アーム動作計画部７０１は、アームの姿勢が把握できており、現在のアームの姿勢が最終目標姿勢に到達していない場合には、アームの微小移動を継続する。アーム動作計画部７０１は、アーム動作計画に基づいてアーム微小移動目標量ＴＡを求める（Ｓ４０６）。アーム微小移動目標量は、例えばアーム動作計画に基づいて定められたアーム１００１の軌道を、所定量ずつ区分したものに相当する。

【0062】

アーム動作計画部７０１は、アーム微小移動目標量ＴＡをアーム動作制御部７０３に指示し、アーム動作制御部７０３は、ロボット１にアーム微小移動目標量ＴＡに応じたアーム動作指示ＣＯＮＴを行う（Ｓ４０７）。これらの制御により、アーム１００１の微小移動が実行される（Ｓ４０８）。本実施例では、アーム動作指示ＣＯＮＴは、アーム１００１各部に供給される液圧である。

【0063】

微小移動実行後、微小移動を反映した最新のアーム姿勢データＡＰを入力し（Ｓ４０３）、アーム１００１の姿勢が把握できているかを判定し（Ｓ４０４）、微小移動を継続する。

【0064】

一方、判断Ｓ４０４にてアームが認識されていない場合、すなわちエラー信号の入力がある場合等では、アーム動作計画部７０１は、アームが以前の状態（例えば直前の状態）に戻るようにアーム微小移動目標量ＴＡを設定し、アーム微小移動目標量ＴＡを制御変数（例えば液圧）に変換し、アーム動作を指示することにより、アーム１００１は以前の状態に戻る（Ｓ４０９）。

【0065】

アーム１００１が直前の状態に戻ることにより、アーム１００１の姿勢を見失う前の状態に復帰することができ、安定した制御が可能となる。また、直前の状態に復帰する以外、所定ステップ前の状態に復帰してもよい。また、アーム１００１の基本姿勢（前述）に復帰してもよい。

【0066】

アーム１００１が前述のように液体の圧力で姿勢を制御されている場合には、直前の状態に戻るためには、例えば前記エラー信号の前後の液圧あるいは液量の差分を検出し、差分にもとづいて制御することができる。また、基本姿勢に復帰するためには、例えば、液圧で駆動される関節のすべてのシリンダーの液圧あるいは液量を最小にすることで制御することができる。この場合には、アーム１００１は例えば最も縮めた状態をとる。また、基本姿勢に復帰するためには、例えば、液圧で駆動される関節のすべてのシリンダーの液圧あるいは液量を最大にすることで制御することができる。この場合には、アーム１００１は例えば最も伸ばした状態をとる。また、基本姿勢に復帰するためには、例えば、液圧で駆動される関節１つ以上のシリンダーの液圧あるいは液量を最大にし、１つ以上のシリンダーの液圧あるいは液量を最小にすることで制御することができる。この場合には、アーム１００１は例えば曲げた状態をとる。

【0067】

一方、アーム動作計画部７０１が生成したアーム微小移動目標量ＴＡは、アーム微小移動目標量履歴７０５として蓄積される。あるアーム微小移動目標量ＴＡによって制御された結果、アーム１００１を見失った場合、すなわちエラー信号を受信した場合等には、アーム微小移動目標量履歴７０５にその履歴を記録する（Ｓ４１０）。

【0068】

判断Ｓ４０４にてアームが認識されていない場合は、処理Ｓ４０９，Ｓ４１０を経て、アーム動作計画の修正を行う。すなわち、例えば、処理４０９で直前の状態に復帰した場合であれば、当該処理４０９で復帰した姿勢を起点として、処理Ｓ４０１で設定した最終目標姿勢ＦＡをとるための動作計画を再作成する。ただし、この動作計画では、アーム１００１を見失う状態を避けるように、動作計画を作成する。そのためには、アーム微小移動目標量履歴７０５を用いて、アーム１００１を見失った直前のアーム微小移動目標量ＴＡを用いないように制御を行う。あるいは、直前のアーム姿勢データＡＰの姿勢を避けるように制御を行ってもよい。

【0069】

また、あるいは、エラー信号の受信の直前に対応したマーカ３次元座標３Ｄを記録して、マーカが見えないエリアの外縁をマッピングすることにより、当該エリアを避けるようにアーム１００１を制御することもできる。

【0070】

本実施例によれば、従来の産業用ロボットに搭載されているような、関節の角度を直接計測するエンコーダなどのセンサを用いずに、アームの姿勢を把握することが可能となる。また、放射線や水蒸気などの影響により、高画質のカメラ画像の取得が期待できない環境下でもアームの姿勢を把握することが可能となる。さらに、周囲の未知の構造物などにより、ロボットやアームを見失う可能性のある状況でも、周囲の構造物との干渉を避け、安全かつ確実にアーム姿勢を制御することができる。

【0071】

本実施例中、ソフトウエアで構成した機能と同等の機能は、FPGA（Field Programmable Gate Array）、ASIC（Application Specific Integrated Circuit）などのハードウェアで置換することでも実現できる。

【0072】

本発明は上記した実施形態に限定されるものではなく、様々な変形例が含まれる。例えば、ある実施例の構成の一部を他の実施例の構成に置き換えることが可能であり、また、ある実施例の構成に他の実施例の構成を加えることが可能である。また、各実施例の構成の一部について、他の実施例の構成の追加・削除・置換をすることが可能である。

【符号の説明】

【0073】

ロボット１、アーム姿勢制御装置４、アーム姿勢推定装置５、画像解析装置６、アーム１００１、移動機構１００２、関節１００３

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】