マニピュレータの制御装置
【課題】手先の目標位置姿勢を指示してから駆動するまでの制御時間の短縮を図りながら、マニピュレータと構造物との衝突回避を図ることができるマニピュレータの制御装置を提供する。
【解決手段】制御装置１４は、多数のマニピュレータ分割表面Ａ１〜Ａｍのうちのいずれか１つと多数の構造物分割表面Ｂ１〜Ｂｎのうちのいずれか１つとの組合せを所定の件数だけランダムに抽出して抽出リストを作成し、その抽出リストの各組合せにおける仮想配置のマニピュレータ分割表面と構造物分割表面との重心間距離を演算し、この重心間距離によりマニピュレータ分割表面と構造物分割表面が衝突する可能性の高いものと判定した組合せからなる候補リストを作成する。そして、候補リストの各組合せにおいて仮想配置のマニピュレータ分割表面と構造物分割表面が衝突するかどうかを判定し、衝突すると判定した場合に、衝突を回避するように関節の目標回転角を補正する。
【解決手段】制御装置１４は、多数のマニピュレータ分割表面Ａ１〜Ａｍのうちのいずれか１つと多数の構造物分割表面Ｂ１〜Ｂｎのうちのいずれか１つとの組合せを所定の件数だけランダムに抽出して抽出リストを作成し、その抽出リストの各組合せにおける仮想配置のマニピュレータ分割表面と構造物分割表面との重心間距離を演算し、この重心間距離によりマニピュレータ分割表面と構造物分割表面が衝突する可能性の高いものと判定した組合せからなる候補リストを作成する。そして、候補リストの各組合せにおいて仮想配置のマニピュレータ分割表面と構造物分割表面が衝突するかどうかを判定し、衝突すると判定した場合に、衝突を回避するように関節の目標回転角を補正する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば原子炉内構造物の検査作業や補修作業に用いるマニピュレータに係わり、特に、マニピュレータを駆動制御する制御装置に関する。
【背景技術】
【０００２】
従来、原子炉内構造物の検査作業や補修作業を目的とし、原子炉内の狭隘部にてマニピュレータを用いた検査装置が提唱されている（例えば、特許文献１参照）。特許文献１に記載の検査装置では、マニピュレータは５軸の回転機構を備えた多関節型椀機構として構成されており、それらの回転機構が制御装置によって駆動制御されている。そして、原子炉内構造物の探傷検査作業を行う場合は、超音波プローブを備えたスキャナをマニピュレータの手先（先端）に取付け、マニピュレータを操作して超音波プローブを検査対象部位の近傍に配置し、スキャナにより超音波プローブを検査対象部位の表面に接触させつつ走査するようになっている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−３４００号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１には記載されていないが、上記制御装置は、例えば、マニピュレータの手先の目標位置姿勢が指示されると、対応する各軸の目標回転角を演算し、この目標回転角となるように回転機構を駆動制御するものと考えられる。また、制御装置は、マニピュレータと構造物との衝突を回避する衝突回避制御を行うことが好ましいと考えられる。この衝突回避制御では、前述のように演算した各軸の目標回転角に基づいてマニピュレータの表面の仮想配置を演算し、この仮想配置のマニピュレータの表面と構造物の表面とが衝突するかどうかを判定し、衝突すると判定した場合に衝突を回避するように各軸の目標回転角を補正し、この補正後の目標回転角となるように回転機構を駆動制御する。ところが、マニピュレータは狭隘部に配置されていることから、マニピュレータと構造物との衝突位置や干渉量を把握するために、マニピュレータの表面及び構造物の表面を小さくとり多数に分割する必要がある。そして、例えば、多数のマニピュレータの分割表面と多数の構造物の分割表面との総当たりの組合せにおいて衝突判定を行うのであれば、その衝突判定に時間がかかってしまう。あるいは、例えば、全てのマニピュレータの分割表面と構造物の分割表面との位置関係から、衝突判定を行うべき組合せを事前に設定するのであれば、その設定に時間がかかってしまう。いずれの場合にしても、マニピュレータの手先の目標位置姿勢を指示してから駆動するまでの制御時間が長くなる。かといって、マニピュレータの表面及び構造物の表面を大きくとり少数に分割すれば、マニピュレータと構造物との衝突位置や干渉量を十分に把握できないため、マニピュレータが構造物に近づけられなくなり、場合によってはマニピュレータが身動きできなくなってしまう。
【０００５】
本発明は、上記事柄に鑑みてなされたものであり、マニピュレータの手先の目標位置姿勢を指示してから駆動するまでの制御時間の短縮を図りながら、マニピュレータと構造物との衝突回避を図ることができるマニピュレータの制御装置を提供することにある。
【課題を解決するための手段】
【０００６】
（１）上記目的を達成するために、本発明は、複数の関節を介し連結された複数のリンクからなるマニピュレータに対し、各関節の回転角を制御することにより手先の位置姿勢を制御する制御装置において、前記マニピュレータに関するデータとして、各リンクの表面及び各関節の表面を多数に分割したマニピュレータ分割表面の形状及びそれらの相対的な位置関係を記憶する第１記憶手段と、前記マニピュレータの周囲の構造物に関するデータとして、前記構造物の表面を多数に分割した構造物分割表面の形状及びそれらの絶対位置を記憶する第２記憶手段と、前記マニピュレータの手元の現在位置姿勢を取得する手元現在位置姿勢取得手段と、前記マニピュレータの手先の目標位置姿勢を入力する手先目標位置姿勢入力手段と、前記手元現在位置姿勢取得手段で取得された手元の現在位置姿勢及び前記手先目標位置姿勢入力手段で入力された手先の目標位置姿勢に基づき、各関節の目標回転角を演算する関節目標回転角演算手段と、前記手元現在位置取得手段で取得された手元の現在位置姿勢、前記関節目標回転角演算手段で演算された各関節の目標回転角、並びに前記第１記憶手段で記憶されたデータに基づき、前記手先の目標位置姿勢に対応する前記多数のマニピュレータ分割表面の仮想配置を演算するマニピュレータ分割表面演算手段と、前記第１記憶手段で記憶された前記多数のマニピュレータ分割表面のうちのいずれか１つと前記第２記憶手段で記憶された前記多数の構造物分割表面のうちのいずれか１つとの組合せを予め設定された所定の件数だけランダムに抽出して抽出リストを作成し、その抽出リストの各組合せにおける前記仮想配置の前記マニピュレータ分割表面と前記構造物分割表面との重心間距離を演算し、この重心間距離が予め設定された所定の閾値未満であるかどうか判定することにより前記マニピュレータ分割表面と前記構造物分割表面が衝突する可能性の高いものであるかどうかを判定し、衝突する可能性の高いものと判定した組合せからなる候補リストを作成する処理を行うリスト作成手段と、前記リスト作成手段で作成された候補リストの各組合せにおいて前記仮想配置の前記マニピュレータ分割表面と前記構造物分割表面が衝突するかどうかを判定する衝突判定手段と、前記衝突判定手段で前記マニピュレータ分割表面と前記構造物分割表面が衝突すると判定された場合に、衝突を回避するように前記関節の目標回転角を補正する関節目標回転角補正手段とを備える。
【０００７】
このように本発明においては、リスト作成手段は、マニピュレータ分割表面と構造物分割表面との組合せをランダムに抽出して抽出リストを作成し、その抽出リストの各組合せにおける仮想配置のマニピュレータ分割表面と構造物分割表面との重心間距離を演算し、この重心間距離が所定の閾値未満であるかどうか判定することにより衝突の可能性の高いものであるかどうかを判定し、衝突の可能性の高いものと判定した組合せからなる候補リストを作成する。衝突判定手段は、候補リストの各組合せにおいて、仮想配置のマニピュレータ分割表面と構造物分割表面が衝突するかどうかを判定する。これにより、マニピュレータと構造物との衝突位置や干渉量を把握するためにマニピュレータ分割表面及び構造物分割表面を小さくとり多数に分割する場合であっても、手先の目標位置姿勢を入力してから駆動するまでの制御時間の短縮を図ることができる。すなわち、例えばマニピュレータ分割表面と構造物分割表面との総当たりの組合せにおいて衝突するかどうかを判定する制御方法と比べ、衝突判定処理時間の短縮を図ることができ、ひいては全体の制御時間の短縮を図ることができる。また、例えば全てのマニピュレータ分割表面と構造物分割表面との位置関係から衝突の可能性の高い組合せからなる候補リストを作成する制御方法と比べ、候補リスト作成処理時間の短縮を図ることができ、ひいては全体の制御時間の短縮を図ることができる。したがって、本発明においては、マニピュレータの手先の目標位置姿勢を指示してから駆動するまでの制御時間の短縮を図りながら、マニピュレータと構造物との衝突回避を図ることができる。
【０００８】
（２）上記（１）において、好ましくは、前記リスト作成手段は、前記処理を予め設定された所定の時間間隔で繰返し行っており、前記抽出リストを作成する際に前回作成した候補リストの組合せを加える。
【０００９】
これにより、例えば手先の目標位置姿勢や現在位置姿勢及び関節の目標回転角や現在回転角などの変化の有無にかかわらず、時間の経過とともに、候補リストにおける組合せの件数、すなわち衝突判定を行う組合せの件数を増加させることができる。その結果、マニピュレータと構造物との衝突回避性を向上させることができる。
【００１０】
（３）上記（１）又は（２）において、好ましくは、前記関節目標回転角補正手段は、前記衝突判定手段で衝突すると判定された前記仮想配置の前記マニピュレータ分割表面の頂点のうち最も前記構造物の内側に位置するものを選択し、この選択した頂点と前記構造物分割表面との距離以上の大きさを持ちかつ前記構造物分割表面の法線ベクトルと同じであって前記構造物の外側への向きを持つ衝突回避ベクトルを生成する衝突回避ベクトル生成手段と、前記衝突判定手段で衝突すると判定された前記マニピュレータ分割表面が、前記複数の関節で区分された複数の部位のいずれに含まれるか判別することにより、前記マニピュレータ分割表面に対応する前記衝突回避ベクトルを分類する分類手段と、前記衝突回避ベクトルを前記部位毎に平均化し、この平均化した衝突回避ベクトルに基づいて前記各関節の目標回転角の補正量を演算する関節目標回転角補正量演算手段とを備える。
【００１１】
（４）上記（１）〜（３）のいずれか１つにおいて、好ましくは、前記マニピュレータの各関節の現在回転角を取得する関節現在回転角取得手段と、前記手元現在位置姿勢取得手段で取得された手元の現在位置姿勢及び前記関節現在回転角取得手段で取得された各関節の現在回転角に基づき、手先の現在位置姿勢を演算する手先現在位置姿勢演算手段とを備え、前記関節目標回転角演算手段は、前記手先目標位置姿勢入力手段で入力された手先の目標位置姿勢と前記手先現在位置姿勢演算手段で演算された手先の現在位置姿勢との偏差を演算し、この位置姿勢の偏差に基づき各関節の目標回転角と現在回転角との偏差を演算し、この各関節回転角の偏差を前記関節現在回転角取得手段で取得された各関節の現在回転角に加算して各関節の目標回転角を演算する。
【発明の効果】
【００１２】
本発明によれば、マニピュレータの手先の目標位置姿勢を指示してから駆動するまでの制御時間の短縮を図りながら、マニピュレータと構造物との衝突回避を図ることができる。
【図面の簡単な説明】
【００１３】
【図１】本発明の一実施形態による原子炉内検査装置の機器配置を一例として表す概略図である。
【図２】本発明の一実施形態におけるマニピュレータの全体構造を関連機器とともに表す概略図である。
【図３】本発明の一実施形態におけるマニピュレータを構成する駆動部の詳細構造を表す断面図である。
【図４】本発明の一実施形態におけるマニピュレータの手先位置姿勢制御に係わる制御装置の機能的構成を表すブロック図である。
【図５】本発明の一実施形態におけるマニピュレータの手先位置姿勢制御に係わる制御装置の制御処理内容を表すＰＡＤ図である。
【図６】本発明の一実施形態における表示装置の設定画面を表す図である。
【図７】本発明の一実施形態における表示装置の操作画面を表す図である。
【図８】図５中の手先の現在位置姿勢の演算処理の詳細を表すＰＡＤ図である。
【図９】図５中の各関節の目標回転角の演算処理の詳細を表すＰＡＤ図である。
【図１０】図５中のリスト作成処理の詳細を表すＰＡＤ図である。
【図１１】本発明の一実施形態におけるマニピュレータ分割表面と構造物分割表面との組合せを説明するための図である。
【図１２】図５中の関節の目標回転角の補正処理の詳細を表すＰＡＤ図である。
【発明を実施するための形態】
【００１４】
以下、本発明の好適な一実施形態である原子炉内検査装置を、図面を参照しつつ説明する。本実施形態の原子炉内検査装置は、原子炉内構造物の目視検査を目的としたものである。
【００１５】
図１は、本実施形態による原子炉内検査装置の機器配置を一例として表す概略図である。なお、本実施形態では、絶対座標系として、図１中紙面に対し垂直な方向をＸ軸、図１中左右方向をＹ軸、図１中上下方向をＺ軸にとっている。
【００１６】
この図１において、原子炉１内には、シュラウド２、上部格子板３、炉心支持板４、シュラウドサポート５、及びＣＲＤ（Control Rod Device：制御棒駆動装置）ハウジング６等の構造物があり、またＰＬＲ(Primary Loop Re-circulation System：一次冷却材再循環系）配管７等の配管が接続されている。原子炉１の上部には、作業スペースであるオペレーションフロア８があり、同じく上方には、燃料交換装置９がある。
【００１７】
本実施形態の原子炉内検査装置は、目視検査用カメラ１０を先端部（手先）に搭載したマニピュレータ１１（水中検査装置）と、このマニピュレータ１１を上下方向に移動可能に収納するガイドパイプ１２と、マニピュレータ１１にケーブル１３を介し接続された制御装置１４と、この制御装置１４に接続され、カメラ１０の映像や位置姿勢等を表示する表示装置１５と、制御装置１４に接続されたコントローラ１６とを備えている。コントローラ１６は、複数の操作レバーを有し、これら操作レバーの操作に応じてカメラ１０（言い換えれば、マニピュレータ１１の手先）のＸ軸方向移動、Ｙ軸方向移動、Ｚ軸方向移動、Ｚ軸周りの回転（ロール）、Ｘ軸周りの回転（ピッチ）、及びＹ軸周りの回転（ヨー）を指示するようになっている。
【００１８】
そして、例えば原子炉１内の構造物の目視検査作業を行う場合は、まず、検査員１７が燃料交換装置９を操作して、マニピュレータ１１をチェーン（図示せず）で吊下げ、マニピュレータ１１及びこれを収納して係合したガイドパイプ１２を原子炉１内に投入する。そして、ガイドパイプ１２を上部格子板３の格子に挿通し炉心支持板４に着座させて、ガイドパイプ１２を位置決めする。その後、燃料交換装置８を操作して、マニピュレータ１１を降下させ、マニピュレータ１１の基端部をガイドパイプ１２の下端部に位置決めする。その後、検査員１７がカメラ１０の映像や位置姿勢等を表示装置１５で確認しつつ、コントローラ１６を操作してマニピュレータ１１を駆動させる。
【００１９】
次に、マニピュレータ１１について説明する。図２は、マニピュレータ１１の全体構造を関連機器とともに表す概略図であり、図３は、マニピュレータ１１を構成する駆動部の詳細構造を表す断面図である。なお、本実施形態では、駆動部毎のローカル座標系として、後述するホルダの回転中心軸とウォームホイールの回転中心軸との交点（２軸の回転中心）を原点とし、図３中紙面に対し垂直な方向をｘ軸、図３中上下方向をｙ軸、図３中左右方向をｚ軸にとっている。
【００２０】
これら図２及び図３において、マニピュレータ１１は、複数（本実施形態では５つ）の駆動部１８を介し連結された複数（本実施形態では６つ）のリンク１９で構成されている。マニピュレータ１１の手先（先端）に位置するリンク１９には目視検査用カメラ１０が設けられている。また、マニピュレータ１１の手元（基端）に位置するリンク１９には信号伝送部２０が設けられ、この信号伝送部２０と制御装置１４との間でケーブル１３が接続されている。
【００２１】
各駆動部１８は、２つの回転機構を有しており、２自由度の関節として構成されている。駆動部１８は、手元側のリンク１９に連結されたホルダ２１と、このホルダ２１に軸受２２を介し回転可能に接続されたボックス２３と、ホルダ２１内に固定されたモータ２４ａと、このモータ２４ａの出力軸に固着され、ボックス２３の歯車穴に嵌合された歯車２５とを備えている。そして、モータ２４ａの回転動力が歯車２５を介しボックス２３に伝達されることにより、ボックス２３（言い換えれば、手先側のリンク１９）がホルダ２１（言い換えれば、手元側のリンク１９）に対して回転する（第１の回転機構）。これにより、ローカル座標系のｚ軸周りの回転角（ロール角）θrを制御するようになっている。なお、モータ２４ａにはエンコーダ２６ａが付設されており、このエンコーダ２６ａは、モータ２４ａの回転角すなわちロール角θｒを検出するようになっている。
【００２２】
また、駆動部１８は、ボックス２３内に回転可能に設けられ、手先側のリンク１９に連結されたウォームホイール２７と、ボックス２３内に回転可能に設けられ、ウォームホイール２７と噛合うウォームギヤ２８と、このウォームギヤ２８と同軸に設けられた平歯車２９ａと、ボックス２３に固定されたモータ２４ｂと、このモータ２４ｂの出力軸に固着され、平歯車２９ａに噛合う平歯車２９ｂとを備えている。そして、モータ２４ｂの回転動力が平歯車２９ａ，２９ｂ及びウォームギヤ２８を介しウォームホイール２７に伝達されることにより、ウォームホイール２７（言い換えれば、手先側のリンク１９）がボックス２３（言い換えれば、手元側のリンク１９）に対して回転する（第２の回転機構）。これにより、ローカル座標系のｘ軸周りの回転角（ピッチ角）θｐを制御するようになっている。なお、モータ２４ｂにはエンコーダ２６ｂが付設されており、このエンコーダ２６ｂは、モータ２４ｂの回転角を検出し、前述したピッチ角θｐを間接的に検出するようになっている。
【００２３】
カメラ１０の映像信号と各駆動部１８のエンコーダ２６ａ，２６ｂ（すなわち、本実施形態では計１０個のエンコーダ）の検出信号は、信号伝送部２０及びケーブル１３を介して制御装置１４に送信されるようになっている。
【００２４】
制御装置１４は、カメラ１０の映像信号を表示装置１５に出力して、カメラ１０の映像を表示させるようになっている。また、制御装置１４は、各駆動部１８のエンコーダ２６ａ，２６ｂの検出信号に基づいて各関節の現在回転角（詳細には、本実施形態では、第１のローカル座標系のロール角θ１ｒ及びピッチ角θ１ｐ、…、第５のローカル座標系のロール角θ５ｒ及びピッチ角θ５ｐ）を演算し、これに基づいてマニピュレータ１１の手先の現在位置姿勢（詳細には、絶対座標系のＸ軸方向位置、Ｙ軸方向位置、Ｚ軸方向位置、ロール角θＲ、ピッチ角θＰ、ヨー角θＹ）を演算し、それらの演算値（現在値）を表示装置１５に出力して表示させ、さらにマニピュレータ１１及びその周囲の構造物の現在画像を構築して表示させるようになっている。
【００２５】
また、制御装置１４は、コントローラ１６からの操作信号に応じてマニピュレータ１１の手先の目標位置姿勢を演算し、これに基づいて各関節の目標回転角を演算し、各関節の目標回転角となるように、ケーブル１３等を介し各駆動部１８のモータ２４ａ，２４ｂに供給する電圧を制御する。これにより、マニピュレータ１１の手先の位置姿勢を制御するようになっている。また、このとき、マニピュレータ１１と構造物との衝突を回避する衝突回避制御も行っており、その制御内容が本実施形態の大きな特徴となっている。
【００２６】
図４は、マニピュレータ１１の手先位置姿勢制御に係わる制御装置１４の機能的構成を表すブロック図である。
【００２７】
この図４において、制御装置１４は、マニピュレータ１１の各駆動部１８のエンコーダ２６ａ，２６ｂの検出信号に基づいて各関節の現在回転角（θ１ｒ，θ１ｐ，θ２ｒ，θ２ｐ，θ３ｒ，θ３ｐ，θ４ｒ，θ４ｐ，θ５ｒ，θ５ｐ）を取得する関節現在回転角取得部３０と、マニピュレータ１１の手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）を取得する手元現在位置姿勢取得部３１と、手元の現在位置姿勢及び各関節の現在回転角に基づいて手先の現在位置姿勢（Ｘ，Ｙ，Ｚ，θＲ，θＰ，θＹ）を演算する手先現在位置姿勢演算部３２とを有している。また、コントローラ１６の操作信号等に基づいて手先の目標位置姿勢（Ｘ_ref，Ｙ_ref，Ｚ_ref，θＲ_ref，θＰ_ref，θＹ_ref）を入力する手先目標位置姿勢入力部３３と、手先の目標位置姿勢と現在位置姿勢との偏差に基づいて各関節の目標回転角（θ１ｒ_ref，θ１ｐ_ref，θ２ｒ_ref，θ２ｐ_ref，θ３ｒ_ref，θ３ｐ_ref，θ４ｒ_ref，θ４ｐ_ref，θ５ｒ_ref，θ５ｐ_ref）を演算する関節目標回転角演算部３４とを有している。
【００２８】
また、制御装置１４は、マニピュレータ１１の設計データ（詳細には、例えば各リンク１９の表面及び各駆動部１８の表面を多数に分割したマニピュレータ分割表面Ａ１〜Ａｍの形状及びそれらの相対的な位置関係等）を記憶するマニピュレータデータ記憶部３５を有している。なお、本実施形態では、マニピュレータ分割表面Ａ１〜Ａｍはそれぞれ三角形状をなし（後述の図１１参照）、各分割表面の３つの頂点の相対座標データが記憶されている。また、マニピュレータ分割表面Ａ１〜Ａｍは、複数の関節で区分された複数の部位のいずれに含まれるか、別の言い方をすれば、複数の駆動部１８のいずれによって回転する部位（具体的には、各部位は、モータ２６ａによって回転するボックス２３、並びにモータ２６ａ，２６ｂによって回転する手先側のリンク１９及びこれに連結されたホルダ２１からなる）であるか分類されて記憶されている。
【００２９】
また、制御装置１４は、原子炉内構造物の設計データ（詳細には、構造物の表面を多数に分割した構造物分割表面Ｂ１〜Ｂｎの形状及びそれらの絶対位置等）を記憶する構造物データ記憶部３６を有している。なお、本実施形態では、構造物分割表面Ｂ１〜Ｂｎはそれぞれ、マニピュレータ分割表面Ａ１〜Ａｍと同程度の大きさであって三角形状をなし（後述の図１１参照）、各分割表面の３つの頂点の絶対座標データが記憶されている。
【００３０】
また、制御装置１４は、マニピュレータ１１の手先の目標位置姿勢に対応する分割表面Ａ１〜Ａｍの仮想配置を演算するマニピュレータ分割表面演算部３７と、衝突判定を行うマニピュレータ分割表面と構造物分割表面との組合せからなる候補リストを作成するリスト作成部３８と、このリスト作成部３８で作成した候補リストを記憶するリスト記憶部３９と、候補リストの各組合せにおいて仮想配置のマニピュレータ分割表面と構造物分割表面が衝突するかどうかを判定する衝突判定部４０と、この衝突判定部４０でマニピュレータ分割表面と構造物分割表面が衝突すると判定された場合に、衝突を回避するように関節の目標回転角を補正する関節目標回転角補正部４１と、この関節目標回転角補正部４１で補正された目標回転角（若しくは、補正の必要がなかった場合は、関節目標回転角演算部３４で演算された目標回転角）となるように、各駆動部１８のモータ２６ａ，２６ｂへ供給する電圧を制御するモータ電圧制御部４２とを有している。
【００３１】
次に、マニピュレータ１１の手先位置姿勢制御に係わる制御装置１４の制御手順を説明する。図５は、制御装置１４の制御処理内容を表すＰＡＤ図である。
【００３２】
まず、ステップ５０にて、制御装置１４は、図６で示すような設定画面を表示装置１５に表示させ、この設定画面の「操作対象データ読込」ボタン４３ａが操作されると（詳細には、例えばマウスの操作により、画面上のカーソルがボタン位置に合わされた状態でクリックされると）、マニピュレータ１１の設計データを外部の記憶装置（図示せず）から取込んでマニピュレータデータ記憶部３５に保存する。また、設定画面の「周囲構造物読込」ボタン４３ｂが操作されると、構造物の設計データ（好ましくは、マニピュレータ１１の存在する領域に対応して選択された構造物の設計データ）を外部の記憶装置から取込んで構造物データ記憶部３６に保存する。そして、ステップ５１に進み、例えばキーボード等の操作により、設定画面の手元現在位置姿勢入力欄４４にマニピュレータ１１の手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）が入力されると、制御装置１４の手元現在位置姿勢取得部３１は、それらの入力値を入力して保存する。なお、マニピュレータ１１の手元の現在位置（Ｘ０，Ｙ０，Ｚ０）は、上部格子板３及び炉心支持板４におけるガイドパイプ１２の設置位置によって求められる。また、本実施形態では、マニピュレータ１１の手元の現在姿勢は、θＲ０＝０、θＰ０＝０、θＹ０＝０である。
【００３３】
その後、ステップ５２に進み、制御装置１４は、図７で示すような操作画面を表示装置１５に表示させ、マニピュレータ操作処理に移る。このマニピュレータ操作処理では、まず、コントローラ１６の操作の有無にかかわらず、ステップ５３の現在値の取得処理を行う。現在値の取得処理では、ステップ５４の各関節の現在回転角の取得処理、及びステップ５５の手先の現在位置姿勢の演算処理を行う。そして、ステップ５６に進み、コントローラ１６が操作されると、その操作に応じて手先の目標位置姿勢の入力処理を行い、その後、ステップ５７に進み、制御値の演算処理を行う。制御値の演算処理では、ステップ５８の各関節の目標回転角の演算処理、ステップ５９のマニピュレータ分割表面の仮想配置の演算処理、ステップ６０のリスト作成処理、ステップ６１の衝突判定処理、及びステップ６２の関節の目標回転角の補正処理を行う。そして、ステップ６３に進み、目標回転角に基づいてモータ電圧の制御処理を行う。前述した処理の詳細を、以下説明する。
【００３４】
（１）各関節の現在回転角の演算処理（ステップ５４）
各関節の現在回転角の演算処理では、制御装置１４の関節現在回転角取得部３０は、各駆動部１８のエンコーダ２６ａ，２６ｂの検出信号に基づいて各関節の現在回転角（θ１ｒ，θ１ｐ，θ２ｒ，θ２ｐ，θ３ｒ，θ３ｐ，θ４ｒ，θ４ｐ，θ５ｒ，θ５ｐ）を演算する。詳細には、各エンコーダの検出信号（パルス信号）をカウントし、これにセンサ固有のスペックとして予め記憶された所定の回転角度変換係数を乗じて、各関節の現在回転角を演算する。そして、演算した各関節の現在回転角は、図示しないＲＡＭ（Random Access Memory：ランダムアクセスメモリ）等に保存するとともに、表示装置１５の操作画面の関節現在回転角表示欄４５に表示させるようになっている。
【００３５】
（２）手先の現在位置姿勢の演算処理（ステップ５５）
図８は、手先の現在位置姿勢の演算処理の詳細を表すＰＡＤ図である。この手先の現在位置姿勢の演算処理では、制御装置１４の手先現在位置姿勢演算部３２は、まず、ステップ６４にて、手元現在位置姿勢取得部３１で取得されたマニピュレータ１１の手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）を読込み、ステップ６５にて、関節現在回転角取得部３０で取得された関節の現在回転角（θ１ｒ，θ１ｐ，θ２ｒ，θ２ｐ，θ３ｒ，θ３ｐ，θ４ｒ，θ４ｐ，θ５ｒ，θ５ｐ）を読込む。そして、ステップ６６に進み、順運動学演算の同次変換行列演算として、関節毎に予め定義された並進移動行列及び回転移動行列を含む行列式を乗算する。この行列式は、一般的にロボット工学等で用いられているものを使用するため、その詳細な説明を省略する。その後、ステップ６７に進み、ステップ６６で得られた行列（４行４列）の成分を用いて手先の現在位置姿勢（Ｘ，Ｙ，Ｚ，θＲ，θＰ，θＹ）を演算し、演算した手先の現在位置姿勢を図示しないＲＡＭ等に保存するとともに、表示装置１５の操作画面の手先現在位置姿勢表示欄４６に表示させるようになっている。
【００３６】
なお、本実施形態では、制御装置１４の画像構築部（図示せず）は、手元現在位置姿勢取得部３２で取得された手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）を読込み、関節現在回転角演算部３０で取得された各関節の現在回転角（θ１ｒ，θ１ｐ，θ２ｒ，θ２ｐ，θ３ｒ，θ３ｐ，θ４ｒ，θ４ｐ，θ５ｒ，θ５ｐ）を読込み、マニピュレータデータ記憶部３５で記憶されたデータを読込み、マニピュレータ分割表面Ａ１〜Ａｍの現在配置を演算する。詳細には、手元の現在位置姿勢及び各関節の現在回転角に基づいてマニピュレータ分割表面の座標変換処理を実施し、マニピュレータ分割表面の絶対位置座標データを生成し、この絶対位置座標データをＲＡＭ等に保存する。また、マニピュレータ分割表面の絶対位置座標データに基づいてマニピュレータ１１の現在画像を構築する。また、マニピュレータ１１の周囲の構造物のデータを構造物データ記憶部３６から読込み、周囲構造物の画像を構築する。そして、マニピュレータの画像と周囲構造物の画像を合成して表示装置１５の操作画面若しくは設定画面のマニピュレータ画像表示領域４７に表示させるようになっている。これにより、検査員１７がマニピュレータ画像表示領域４７を見て、構造物に対するマニピュレータ１１の現在配置を確認可能としている。
【００３７】
（３）手先の目標位置姿勢の入力処理（ステップ５６）
手先の目標位置姿勢入力処理では、制御装置１４の手先目標位置姿勢入力部３３は、まず、手元現在位置姿勢取得部３１で取得されたマニピュレータ１１の手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）を読込む。そして、コントローラ１６の操作信号を手先の移動回転方向及び移動回転量に換算し、これを手元の現在位置姿勢に加算して目標位置姿勢（Ｘ_ref，Ｙ_ref，Ｚ_ref，θＲ_ref，θＰ_ref，θＹ_ref）を演算する。そして、演算した手先の目標位置姿勢をＲＡＭ等に保存するとともに、表示装置１５の操作画面の手先目標位置姿勢表示欄４８に表示させるようになっている。
【００３８】
（４）各関節の目標回転角の演算処理（ステップ５８）
図９は、各関節の目標回転角の演算処理の詳細を表すＰＡＤ図である。各関節の目標回転角の演算処理では、制御装置１４の関節目標回転角演算部３４は、まず、ステップ６８にて、手先目標位置姿勢入力部３３で入力された手先の目標位置姿勢を読込み、ステップ６９にて、手元現在位置姿勢取得部３１で取得されたマニピュレータ１１の手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）を読込み、ステップ７０にて、関節現在回転角取得部３０で取得された関節の目標回転角（θ１ｒ，θ１ｐ，θ２ｒ，θ２ｐ，θ３ｒ，θ３ｐ，θ４ｒ，θ４ｐ，θ５ｒ，θ５ｐ）を読込む。
【００３９】
そして、ステップ７１に進み、各関節の目標回転角と現在回転角との偏差Δθから手先の目標位置姿勢と現在位置姿勢との偏差ΔＰを演算する際に用いるヤゴビ行列Ｊを算出し（下記の式（１）参照）、ステップ７２に進み、そのヤコビ行列の逆行列Ｊ−１を算出する。ここで、本実施形態のマニピュレータ１１の動作自由度は１０であって、手先の位置姿勢の成分数の６を超えた冗長系であるから、ヤコビ行列Ｊは正方行列ではない。そのため、転置行列ＪＴを用いて疑似逆行列演算を行い、ヤコビ行列の逆行列Ｊ−１を算出する（下記の式（２）参照）。
【００４０】
【数１】
【００４１】
【数２】
【００４２】
【数３】
【００４３】
そして、ステップ７３に進み、手先の目標位置姿勢と現在位置姿勢との偏差ΔＰから各関節の目標回転角と現在回転角との偏差Δθを演算し（上記の式（３）参照）、この偏差Δθをステップ６９で読込んだ各関節の現在回転角に加算して目標回転角（θ１ｒ_ref，θ１ｐ_ref，θ２ｒ_ref，θ２ｐ_ref，θ３ｒ_ref，θ３ｐ_ref，θ４ｒ_ref，θ４ｐ_ref，θ５ｒ_ref，θ５ｐ_ref）を演算する。そして、演算した各関節の目標回転角をＲＡＭ等に保存するとともに、表示装置１５の操作画面の関節目標回転角表示欄４９に表示させるようになっている。
【００４４】
（５）マニピュレータ分割表面の仮想配置の演算処理（ステップ５９）
マニピュレータ分割表面の仮想配置の演算処理では、制御装置１４のマニピュレータ分割表面演算部３７は、手元現在位置姿勢取得部３２で取得された手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）を読込み、関節目標回転角演算部３４で演算された各関節の目標回転角（θ１ｒ_ref，θ１ｐ_ref，θ２ｒ_ref，θ２ｐ_ref，θ３ｒ_ref，θ３ｐ_ref，θ４ｒ_ref，θ４ｐ_ref，θ５ｒ_ref，θ５ｐ_ref）を読込み、マニピュレータデータ記憶部３５で記憶されたデータを読込み、マニピュレータ分割表面Ａ１〜Ａｍの仮想配置を演算する。詳細には、手元の現在位置姿勢及び各関節の目標回転角に基づいてマニピュレータ分割表面の座標変換処理を実施し、マニピュレータ分割表面の絶対位置座標データを生成し、この絶対位置座標データをＲＡＭ等に保存する。
【００４５】
（６）リスト作成処理（ステップ６０）
図１０は、リスト作成処理の詳細を表すＰＡＤ図である。リスト作成処理では、制御装置１４のリスト作成部３８は、まず、ステップ７４にて、マニピュレータデータ記憶部３５で記憶された多数のマニピュレータ分割表面Ａ１〜Ａｍのうちのいずれか１つと構造物データ記憶部３６で記憶された多数の構造物分割表面Ｂ１〜Ｂｎのうちのいずれか１つとの組合せ（図１１参照）を、予め設定された所定の件数（例えば総数の１０％程度）だけランダムに抽出して、抽出リストを作成する。そして、最初のリスト作成処理ではステップ７５（詳細は後述）をとばし、２回目以降のリスト作成処理ではステップ７５を経由してステップ７６に進み、抽出リストに含まれるマニピュレータ分割表面に対し、前述のステップ５９で演算された仮想配置を読込む。また、ステップ７７に進み、抽出リストに含まれる構造物分割表面に対し、構造物データ記憶部３６から絶対位置を読込む。その後、ステップ７８に進み、抽出リストの各組合せにおける仮想配置のマニピュレータ分割表面と構造物分割表面との重心間距離を演算し、ステップ７９に進み、この重心間距離が予め設定された所定の閾値未満であるかどうか判定することによりマニピュレータ分割表面と構造物分割表面が衝突する可能性の高いものであるかどうかを判定する。そして、ステップ８０に進み、衝突する可能性の高いものと判定した組合せからなる候補リストを作成し、その後、ステップ８１に進み、作成した候補リストをリスト記憶部３９に更新して記憶する。
【００４６】
上述したステップ７４〜７８からなるリスト作成処理は、他の処理（衝突判定処理等）のタイミングとは異なり、予め設定された所定の時間間隔（衝突判定処理を行う時間間隔より短い時間間隔）で繰返し行っている。そして、ステップ７５では、前回作成した候補リストの組合せをリスト記憶部３９から読込み、ステップ７４で作成した抽出リストに追加するようになっている。
【００４７】
（７）衝突判定処理（ステップ６１）
衝突判定処理では、制御装置１４の衝突判定部４０は、候補リストに含まれるマニピュレータ分割表面に対し、前述のステップ５９で演算された仮想配置を読込む。また、候補リストに含まれる構造物分割表面に対し、構造物データ記憶部３６から絶対位置を読込む。その後、候補リストの各組合せにおいて、仮想配置のマニピュレータ分割表面と構造物分割表面が衝突するかどうかを、公知の交差判定によって判定するようになっている。
【００４８】
（８）関節の目標回転角の補正処理（ステップ６２）
図１２は、関節の目標回転角の補正処理の詳細を表すＰＡＤ図である。関節の目標回転角の補正処理では、制御装置１４の関節目標回転角補正部４１は、ステップ８２にて、ステップ６１で衝突すると判定された各組合せにおいて、仮想配置のマニピュレータ分割表面と構造部分割表面との位置関係から、仮想配置のマニピュレータ分割表面における頂点のうち最も構造物内側に位置するものを選択し、この頂点と構造物分割表面との距離を演算し、この距離以上の大きさを持ちかつ構造物分割表面の法線ベクトルと同じであって構造物の外側への向きを持つ衝突回避ベクトルを生成する。その後、ステップ８３に進み、マニピュレータ分割表面が複数の関節で区分された複数の部位のいずれに含まれるか判別することにより、ステップ８２で作成した衝突回避ベクトルを分類する。そして、ステップ８３に進み、部位毎に衝突回避ベクトルが１つである場合はそのままとし、複数ある場合は、ステップ８４に進み、衝突回避ベクトルを平均化する。その後、ステップ８５に進み、平均化した衝突回避ベクトルに基づいて関節の目標回転角の補正量を演算し、この補正量をステップ５８で演算された目標回転角に加算する。
【００４９】
以上のように構成された本実施形態においては、制御装置１４は、リスト作成処理にて、マニピュレータ分割表面と構造物分割表面との組合せをランダムに抽出して抽出リストを作成し、その抽出リストの各組合せにおける仮想配置のマニピュレータ分割表面と構造物分割表面との重心間距離を演算し、この重心間距離が所定の閾値未満であるかどうか判定することにより衝突の可能性の高いものであるかどうかを判定し、衝突の可能性の高いものと判定した組合せからなる候補リストを作成する。そして、衝突判定処理にて、候補リストの各組合せにおいて仮想配置のマニピュレータ分割表面と構造物分割表面が衝突するかどうかを判定する。これにより、マニピュレータ１１と構造物との衝突位置や干渉量を把握するためにマニピュレータ分割表面及び構造物分割表面を小さくとり多数に分割する場合であっても、コントローラ１６を操作してから（言い換えれば、マニピュレータ１１の手先の目標位置姿勢を入力してから）マニピュレータ１１が駆動するまでの制御時間の短縮を図ることができる。すなわち、例えばマニピュレータ分割表面と構造物分割表面との総当たりの組合せにおいて衝突するかどうかを判定する制御方法と比べ、衝突判定処理時間の短縮を図ることができ、ひいては全体の制御時間の短縮を図ることができる。また、例えば全てのマニピュレータ分割表面と構造物分割表面との位置関係から衝突の可能性の高い組合せからなる候補リストを作成する制御方法と比べ、候補リスト作成処理時間の短縮を図ることができ、ひいては全体の制御時間の短縮を図ることができる。したがって、本実施形態においては、マニピュレータ１１の手先の目標位置姿勢を指示してから駆動するまでの制御時間の短縮を図りながら、マニピュレータ１１と構造物との衝突回避を図ることができる。
【００５０】
また、本実施形態においては、制御装置１４は、リスト作成処理を所定の時間間隔で繰返し行っており、抽出リストを作成する際に前回作成した候補リストの組合せを加える。これにより、例えば手先の目標位置姿勢や現在位置姿勢及び関節の目標回転角や現在回転角などの変化の有無にかかわらず、時間の経過とともに、候補リストにおける組合せの件数、すなわち衝突判定を行う組合せの件数を増加させることができる。その結果、マニピュレータ１１と構造物との衝突回避性を向上させることができる。
【００５１】
なお、上記一実施形態においては、マニピュレータ分割表面Ａ１〜Ａｍ及び構造物分割表面Ｂ１〜Ｂｎはそれぞれ三角形状をなす場合を例にとって説明したが、これに限られず、四角形等の他の形状をなしてもよいし、複数種類の形状が混在してもよい。このような場合も、上記同様の効果を得ることができる。
【００５２】
また、上記一実施形態においては、表示装置１４は、マニピュレータ１１の現在画像及び周囲構造物の現在画像を構築して表示装置１５の設定画面若しくは操作画面のマニピュレータ画像表示領域４７に表示させる場合を例にとって説明したが、これに限られない。すなわち、例えば原子炉内にマニピュレータ観察用カメラを配置し、このカメラの映像をマニピュレータ画像表示領域４７に表示させてもよい。この場合も、上記同様の効果を得ることができる。
【００５３】
なお、以上においては、本発明の適用対象として、原子炉内構造物の目視検査作業を目的とし、マニピュレータ１１の先端部に目視検査用カメラ１０を搭載した原子炉内検査装置を例にとって説明したが、これに限られない。すなわち、原子炉内構造物に発生した欠陥の長さ及び深さを測定する探傷検査作業を目的とし、マニピュレータ１１の先端部に渦電流センサ又は超音波センサを搭載した原子炉内検査装置に適用してもよい。また、原子炉内構造物の補修作業を目的とし、マニピュレータ１１の先端部に補修器具を搭載した原子炉内補修装置に適用してもよい。また、原子炉以外の狭隘部にてマニピュレータを用いる装置に適用してもよい。その際、原子炉のように水中にてマニピュレータを用いる装置に限られず、気中にてマニピュレータを用いる装置に適用してもよい。これらの場合も、上記同様の効果を得ることができる。
【符号の説明】
【００５４】
１１ マニピュレータ
１４ 制御装置
１６ コントローラ
１８ 駆動部
１９ リンク
２４ａ，２４ｂ モータ
２６ａ，２６ｂ エンコーダ
【技術分野】
【０００１】
本発明は、例えば原子炉内構造物の検査作業や補修作業に用いるマニピュレータに係わり、特に、マニピュレータを駆動制御する制御装置に関する。
【背景技術】
【０００２】
従来、原子炉内構造物の検査作業や補修作業を目的とし、原子炉内の狭隘部にてマニピュレータを用いた検査装置が提唱されている（例えば、特許文献１参照）。特許文献１に記載の検査装置では、マニピュレータは５軸の回転機構を備えた多関節型椀機構として構成されており、それらの回転機構が制御装置によって駆動制御されている。そして、原子炉内構造物の探傷検査作業を行う場合は、超音波プローブを備えたスキャナをマニピュレータの手先（先端）に取付け、マニピュレータを操作して超音波プローブを検査対象部位の近傍に配置し、スキャナにより超音波プローブを検査対象部位の表面に接触させつつ走査するようになっている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−３４００号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１には記載されていないが、上記制御装置は、例えば、マニピュレータの手先の目標位置姿勢が指示されると、対応する各軸の目標回転角を演算し、この目標回転角となるように回転機構を駆動制御するものと考えられる。また、制御装置は、マニピュレータと構造物との衝突を回避する衝突回避制御を行うことが好ましいと考えられる。この衝突回避制御では、前述のように演算した各軸の目標回転角に基づいてマニピュレータの表面の仮想配置を演算し、この仮想配置のマニピュレータの表面と構造物の表面とが衝突するかどうかを判定し、衝突すると判定した場合に衝突を回避するように各軸の目標回転角を補正し、この補正後の目標回転角となるように回転機構を駆動制御する。ところが、マニピュレータは狭隘部に配置されていることから、マニピュレータと構造物との衝突位置や干渉量を把握するために、マニピュレータの表面及び構造物の表面を小さくとり多数に分割する必要がある。そして、例えば、多数のマニピュレータの分割表面と多数の構造物の分割表面との総当たりの組合せにおいて衝突判定を行うのであれば、その衝突判定に時間がかかってしまう。あるいは、例えば、全てのマニピュレータの分割表面と構造物の分割表面との位置関係から、衝突判定を行うべき組合せを事前に設定するのであれば、その設定に時間がかかってしまう。いずれの場合にしても、マニピュレータの手先の目標位置姿勢を指示してから駆動するまでの制御時間が長くなる。かといって、マニピュレータの表面及び構造物の表面を大きくとり少数に分割すれば、マニピュレータと構造物との衝突位置や干渉量を十分に把握できないため、マニピュレータが構造物に近づけられなくなり、場合によってはマニピュレータが身動きできなくなってしまう。
【０００５】
本発明は、上記事柄に鑑みてなされたものであり、マニピュレータの手先の目標位置姿勢を指示してから駆動するまでの制御時間の短縮を図りながら、マニピュレータと構造物との衝突回避を図ることができるマニピュレータの制御装置を提供することにある。
【課題を解決するための手段】
【０００６】
（１）上記目的を達成するために、本発明は、複数の関節を介し連結された複数のリンクからなるマニピュレータに対し、各関節の回転角を制御することにより手先の位置姿勢を制御する制御装置において、前記マニピュレータに関するデータとして、各リンクの表面及び各関節の表面を多数に分割したマニピュレータ分割表面の形状及びそれらの相対的な位置関係を記憶する第１記憶手段と、前記マニピュレータの周囲の構造物に関するデータとして、前記構造物の表面を多数に分割した構造物分割表面の形状及びそれらの絶対位置を記憶する第２記憶手段と、前記マニピュレータの手元の現在位置姿勢を取得する手元現在位置姿勢取得手段と、前記マニピュレータの手先の目標位置姿勢を入力する手先目標位置姿勢入力手段と、前記手元現在位置姿勢取得手段で取得された手元の現在位置姿勢及び前記手先目標位置姿勢入力手段で入力された手先の目標位置姿勢に基づき、各関節の目標回転角を演算する関節目標回転角演算手段と、前記手元現在位置取得手段で取得された手元の現在位置姿勢、前記関節目標回転角演算手段で演算された各関節の目標回転角、並びに前記第１記憶手段で記憶されたデータに基づき、前記手先の目標位置姿勢に対応する前記多数のマニピュレータ分割表面の仮想配置を演算するマニピュレータ分割表面演算手段と、前記第１記憶手段で記憶された前記多数のマニピュレータ分割表面のうちのいずれか１つと前記第２記憶手段で記憶された前記多数の構造物分割表面のうちのいずれか１つとの組合せを予め設定された所定の件数だけランダムに抽出して抽出リストを作成し、その抽出リストの各組合せにおける前記仮想配置の前記マニピュレータ分割表面と前記構造物分割表面との重心間距離を演算し、この重心間距離が予め設定された所定の閾値未満であるかどうか判定することにより前記マニピュレータ分割表面と前記構造物分割表面が衝突する可能性の高いものであるかどうかを判定し、衝突する可能性の高いものと判定した組合せからなる候補リストを作成する処理を行うリスト作成手段と、前記リスト作成手段で作成された候補リストの各組合せにおいて前記仮想配置の前記マニピュレータ分割表面と前記構造物分割表面が衝突するかどうかを判定する衝突判定手段と、前記衝突判定手段で前記マニピュレータ分割表面と前記構造物分割表面が衝突すると判定された場合に、衝突を回避するように前記関節の目標回転角を補正する関節目標回転角補正手段とを備える。
【０００７】
このように本発明においては、リスト作成手段は、マニピュレータ分割表面と構造物分割表面との組合せをランダムに抽出して抽出リストを作成し、その抽出リストの各組合せにおける仮想配置のマニピュレータ分割表面と構造物分割表面との重心間距離を演算し、この重心間距離が所定の閾値未満であるかどうか判定することにより衝突の可能性の高いものであるかどうかを判定し、衝突の可能性の高いものと判定した組合せからなる候補リストを作成する。衝突判定手段は、候補リストの各組合せにおいて、仮想配置のマニピュレータ分割表面と構造物分割表面が衝突するかどうかを判定する。これにより、マニピュレータと構造物との衝突位置や干渉量を把握するためにマニピュレータ分割表面及び構造物分割表面を小さくとり多数に分割する場合であっても、手先の目標位置姿勢を入力してから駆動するまでの制御時間の短縮を図ることができる。すなわち、例えばマニピュレータ分割表面と構造物分割表面との総当たりの組合せにおいて衝突するかどうかを判定する制御方法と比べ、衝突判定処理時間の短縮を図ることができ、ひいては全体の制御時間の短縮を図ることができる。また、例えば全てのマニピュレータ分割表面と構造物分割表面との位置関係から衝突の可能性の高い組合せからなる候補リストを作成する制御方法と比べ、候補リスト作成処理時間の短縮を図ることができ、ひいては全体の制御時間の短縮を図ることができる。したがって、本発明においては、マニピュレータの手先の目標位置姿勢を指示してから駆動するまでの制御時間の短縮を図りながら、マニピュレータと構造物との衝突回避を図ることができる。
【０００８】
（２）上記（１）において、好ましくは、前記リスト作成手段は、前記処理を予め設定された所定の時間間隔で繰返し行っており、前記抽出リストを作成する際に前回作成した候補リストの組合せを加える。
【０００９】
これにより、例えば手先の目標位置姿勢や現在位置姿勢及び関節の目標回転角や現在回転角などの変化の有無にかかわらず、時間の経過とともに、候補リストにおける組合せの件数、すなわち衝突判定を行う組合せの件数を増加させることができる。その結果、マニピュレータと構造物との衝突回避性を向上させることができる。
【００１０】
（３）上記（１）又は（２）において、好ましくは、前記関節目標回転角補正手段は、前記衝突判定手段で衝突すると判定された前記仮想配置の前記マニピュレータ分割表面の頂点のうち最も前記構造物の内側に位置するものを選択し、この選択した頂点と前記構造物分割表面との距離以上の大きさを持ちかつ前記構造物分割表面の法線ベクトルと同じであって前記構造物の外側への向きを持つ衝突回避ベクトルを生成する衝突回避ベクトル生成手段と、前記衝突判定手段で衝突すると判定された前記マニピュレータ分割表面が、前記複数の関節で区分された複数の部位のいずれに含まれるか判別することにより、前記マニピュレータ分割表面に対応する前記衝突回避ベクトルを分類する分類手段と、前記衝突回避ベクトルを前記部位毎に平均化し、この平均化した衝突回避ベクトルに基づいて前記各関節の目標回転角の補正量を演算する関節目標回転角補正量演算手段とを備える。
【００１１】
（４）上記（１）〜（３）のいずれか１つにおいて、好ましくは、前記マニピュレータの各関節の現在回転角を取得する関節現在回転角取得手段と、前記手元現在位置姿勢取得手段で取得された手元の現在位置姿勢及び前記関節現在回転角取得手段で取得された各関節の現在回転角に基づき、手先の現在位置姿勢を演算する手先現在位置姿勢演算手段とを備え、前記関節目標回転角演算手段は、前記手先目標位置姿勢入力手段で入力された手先の目標位置姿勢と前記手先現在位置姿勢演算手段で演算された手先の現在位置姿勢との偏差を演算し、この位置姿勢の偏差に基づき各関節の目標回転角と現在回転角との偏差を演算し、この各関節回転角の偏差を前記関節現在回転角取得手段で取得された各関節の現在回転角に加算して各関節の目標回転角を演算する。
【発明の効果】
【００１２】
本発明によれば、マニピュレータの手先の目標位置姿勢を指示してから駆動するまでの制御時間の短縮を図りながら、マニピュレータと構造物との衝突回避を図ることができる。
【図面の簡単な説明】
【００１３】
【図１】本発明の一実施形態による原子炉内検査装置の機器配置を一例として表す概略図である。
【図２】本発明の一実施形態におけるマニピュレータの全体構造を関連機器とともに表す概略図である。
【図３】本発明の一実施形態におけるマニピュレータを構成する駆動部の詳細構造を表す断面図である。
【図４】本発明の一実施形態におけるマニピュレータの手先位置姿勢制御に係わる制御装置の機能的構成を表すブロック図である。
【図５】本発明の一実施形態におけるマニピュレータの手先位置姿勢制御に係わる制御装置の制御処理内容を表すＰＡＤ図である。
【図６】本発明の一実施形態における表示装置の設定画面を表す図である。
【図７】本発明の一実施形態における表示装置の操作画面を表す図である。
【図８】図５中の手先の現在位置姿勢の演算処理の詳細を表すＰＡＤ図である。
【図９】図５中の各関節の目標回転角の演算処理の詳細を表すＰＡＤ図である。
【図１０】図５中のリスト作成処理の詳細を表すＰＡＤ図である。
【図１１】本発明の一実施形態におけるマニピュレータ分割表面と構造物分割表面との組合せを説明するための図である。
【図１２】図５中の関節の目標回転角の補正処理の詳細を表すＰＡＤ図である。
【発明を実施するための形態】
【００１４】
以下、本発明の好適な一実施形態である原子炉内検査装置を、図面を参照しつつ説明する。本実施形態の原子炉内検査装置は、原子炉内構造物の目視検査を目的としたものである。
【００１５】
図１は、本実施形態による原子炉内検査装置の機器配置を一例として表す概略図である。なお、本実施形態では、絶対座標系として、図１中紙面に対し垂直な方向をＸ軸、図１中左右方向をＹ軸、図１中上下方向をＺ軸にとっている。
【００１６】
この図１において、原子炉１内には、シュラウド２、上部格子板３、炉心支持板４、シュラウドサポート５、及びＣＲＤ（Control Rod Device：制御棒駆動装置）ハウジング６等の構造物があり、またＰＬＲ(Primary Loop Re-circulation System：一次冷却材再循環系）配管７等の配管が接続されている。原子炉１の上部には、作業スペースであるオペレーションフロア８があり、同じく上方には、燃料交換装置９がある。
【００１７】
本実施形態の原子炉内検査装置は、目視検査用カメラ１０を先端部（手先）に搭載したマニピュレータ１１（水中検査装置）と、このマニピュレータ１１を上下方向に移動可能に収納するガイドパイプ１２と、マニピュレータ１１にケーブル１３を介し接続された制御装置１４と、この制御装置１４に接続され、カメラ１０の映像や位置姿勢等を表示する表示装置１５と、制御装置１４に接続されたコントローラ１６とを備えている。コントローラ１６は、複数の操作レバーを有し、これら操作レバーの操作に応じてカメラ１０（言い換えれば、マニピュレータ１１の手先）のＸ軸方向移動、Ｙ軸方向移動、Ｚ軸方向移動、Ｚ軸周りの回転（ロール）、Ｘ軸周りの回転（ピッチ）、及びＹ軸周りの回転（ヨー）を指示するようになっている。
【００１８】
そして、例えば原子炉１内の構造物の目視検査作業を行う場合は、まず、検査員１７が燃料交換装置９を操作して、マニピュレータ１１をチェーン（図示せず）で吊下げ、マニピュレータ１１及びこれを収納して係合したガイドパイプ１２を原子炉１内に投入する。そして、ガイドパイプ１２を上部格子板３の格子に挿通し炉心支持板４に着座させて、ガイドパイプ１２を位置決めする。その後、燃料交換装置８を操作して、マニピュレータ１１を降下させ、マニピュレータ１１の基端部をガイドパイプ１２の下端部に位置決めする。その後、検査員１７がカメラ１０の映像や位置姿勢等を表示装置１５で確認しつつ、コントローラ１６を操作してマニピュレータ１１を駆動させる。
【００１９】
次に、マニピュレータ１１について説明する。図２は、マニピュレータ１１の全体構造を関連機器とともに表す概略図であり、図３は、マニピュレータ１１を構成する駆動部の詳細構造を表す断面図である。なお、本実施形態では、駆動部毎のローカル座標系として、後述するホルダの回転中心軸とウォームホイールの回転中心軸との交点（２軸の回転中心）を原点とし、図３中紙面に対し垂直な方向をｘ軸、図３中上下方向をｙ軸、図３中左右方向をｚ軸にとっている。
【００２０】
これら図２及び図３において、マニピュレータ１１は、複数（本実施形態では５つ）の駆動部１８を介し連結された複数（本実施形態では６つ）のリンク１９で構成されている。マニピュレータ１１の手先（先端）に位置するリンク１９には目視検査用カメラ１０が設けられている。また、マニピュレータ１１の手元（基端）に位置するリンク１９には信号伝送部２０が設けられ、この信号伝送部２０と制御装置１４との間でケーブル１３が接続されている。
【００２１】
各駆動部１８は、２つの回転機構を有しており、２自由度の関節として構成されている。駆動部１８は、手元側のリンク１９に連結されたホルダ２１と、このホルダ２１に軸受２２を介し回転可能に接続されたボックス２３と、ホルダ２１内に固定されたモータ２４ａと、このモータ２４ａの出力軸に固着され、ボックス２３の歯車穴に嵌合された歯車２５とを備えている。そして、モータ２４ａの回転動力が歯車２５を介しボックス２３に伝達されることにより、ボックス２３（言い換えれば、手先側のリンク１９）がホルダ２１（言い換えれば、手元側のリンク１９）に対して回転する（第１の回転機構）。これにより、ローカル座標系のｚ軸周りの回転角（ロール角）θrを制御するようになっている。なお、モータ２４ａにはエンコーダ２６ａが付設されており、このエンコーダ２６ａは、モータ２４ａの回転角すなわちロール角θｒを検出するようになっている。
【００２２】
また、駆動部１８は、ボックス２３内に回転可能に設けられ、手先側のリンク１９に連結されたウォームホイール２７と、ボックス２３内に回転可能に設けられ、ウォームホイール２７と噛合うウォームギヤ２８と、このウォームギヤ２８と同軸に設けられた平歯車２９ａと、ボックス２３に固定されたモータ２４ｂと、このモータ２４ｂの出力軸に固着され、平歯車２９ａに噛合う平歯車２９ｂとを備えている。そして、モータ２４ｂの回転動力が平歯車２９ａ，２９ｂ及びウォームギヤ２８を介しウォームホイール２７に伝達されることにより、ウォームホイール２７（言い換えれば、手先側のリンク１９）がボックス２３（言い換えれば、手元側のリンク１９）に対して回転する（第２の回転機構）。これにより、ローカル座標系のｘ軸周りの回転角（ピッチ角）θｐを制御するようになっている。なお、モータ２４ｂにはエンコーダ２６ｂが付設されており、このエンコーダ２６ｂは、モータ２４ｂの回転角を検出し、前述したピッチ角θｐを間接的に検出するようになっている。
【００２３】
カメラ１０の映像信号と各駆動部１８のエンコーダ２６ａ，２６ｂ（すなわち、本実施形態では計１０個のエンコーダ）の検出信号は、信号伝送部２０及びケーブル１３を介して制御装置１４に送信されるようになっている。
【００２４】
制御装置１４は、カメラ１０の映像信号を表示装置１５に出力して、カメラ１０の映像を表示させるようになっている。また、制御装置１４は、各駆動部１８のエンコーダ２６ａ，２６ｂの検出信号に基づいて各関節の現在回転角（詳細には、本実施形態では、第１のローカル座標系のロール角θ１ｒ及びピッチ角θ１ｐ、…、第５のローカル座標系のロール角θ５ｒ及びピッチ角θ５ｐ）を演算し、これに基づいてマニピュレータ１１の手先の現在位置姿勢（詳細には、絶対座標系のＸ軸方向位置、Ｙ軸方向位置、Ｚ軸方向位置、ロール角θＲ、ピッチ角θＰ、ヨー角θＹ）を演算し、それらの演算値（現在値）を表示装置１５に出力して表示させ、さらにマニピュレータ１１及びその周囲の構造物の現在画像を構築して表示させるようになっている。
【００２５】
また、制御装置１４は、コントローラ１６からの操作信号に応じてマニピュレータ１１の手先の目標位置姿勢を演算し、これに基づいて各関節の目標回転角を演算し、各関節の目標回転角となるように、ケーブル１３等を介し各駆動部１８のモータ２４ａ，２４ｂに供給する電圧を制御する。これにより、マニピュレータ１１の手先の位置姿勢を制御するようになっている。また、このとき、マニピュレータ１１と構造物との衝突を回避する衝突回避制御も行っており、その制御内容が本実施形態の大きな特徴となっている。
【００２６】
図４は、マニピュレータ１１の手先位置姿勢制御に係わる制御装置１４の機能的構成を表すブロック図である。
【００２７】
この図４において、制御装置１４は、マニピュレータ１１の各駆動部１８のエンコーダ２６ａ，２６ｂの検出信号に基づいて各関節の現在回転角（θ１ｒ，θ１ｐ，θ２ｒ，θ２ｐ，θ３ｒ，θ３ｐ，θ４ｒ，θ４ｐ，θ５ｒ，θ５ｐ）を取得する関節現在回転角取得部３０と、マニピュレータ１１の手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）を取得する手元現在位置姿勢取得部３１と、手元の現在位置姿勢及び各関節の現在回転角に基づいて手先の現在位置姿勢（Ｘ，Ｙ，Ｚ，θＲ，θＰ，θＹ）を演算する手先現在位置姿勢演算部３２とを有している。また、コントローラ１６の操作信号等に基づいて手先の目標位置姿勢（Ｘ_ref，Ｙ_ref，Ｚ_ref，θＲ_ref，θＰ_ref，θＹ_ref）を入力する手先目標位置姿勢入力部３３と、手先の目標位置姿勢と現在位置姿勢との偏差に基づいて各関節の目標回転角（θ１ｒ_ref，θ１ｐ_ref，θ２ｒ_ref，θ２ｐ_ref，θ３ｒ_ref，θ３ｐ_ref，θ４ｒ_ref，θ４ｐ_ref，θ５ｒ_ref，θ５ｐ_ref）を演算する関節目標回転角演算部３４とを有している。
【００２８】
また、制御装置１４は、マニピュレータ１１の設計データ（詳細には、例えば各リンク１９の表面及び各駆動部１８の表面を多数に分割したマニピュレータ分割表面Ａ１〜Ａｍの形状及びそれらの相対的な位置関係等）を記憶するマニピュレータデータ記憶部３５を有している。なお、本実施形態では、マニピュレータ分割表面Ａ１〜Ａｍはそれぞれ三角形状をなし（後述の図１１参照）、各分割表面の３つの頂点の相対座標データが記憶されている。また、マニピュレータ分割表面Ａ１〜Ａｍは、複数の関節で区分された複数の部位のいずれに含まれるか、別の言い方をすれば、複数の駆動部１８のいずれによって回転する部位（具体的には、各部位は、モータ２６ａによって回転するボックス２３、並びにモータ２６ａ，２６ｂによって回転する手先側のリンク１９及びこれに連結されたホルダ２１からなる）であるか分類されて記憶されている。
【００２９】
また、制御装置１４は、原子炉内構造物の設計データ（詳細には、構造物の表面を多数に分割した構造物分割表面Ｂ１〜Ｂｎの形状及びそれらの絶対位置等）を記憶する構造物データ記憶部３６を有している。なお、本実施形態では、構造物分割表面Ｂ１〜Ｂｎはそれぞれ、マニピュレータ分割表面Ａ１〜Ａｍと同程度の大きさであって三角形状をなし（後述の図１１参照）、各分割表面の３つの頂点の絶対座標データが記憶されている。
【００３０】
また、制御装置１４は、マニピュレータ１１の手先の目標位置姿勢に対応する分割表面Ａ１〜Ａｍの仮想配置を演算するマニピュレータ分割表面演算部３７と、衝突判定を行うマニピュレータ分割表面と構造物分割表面との組合せからなる候補リストを作成するリスト作成部３８と、このリスト作成部３８で作成した候補リストを記憶するリスト記憶部３９と、候補リストの各組合せにおいて仮想配置のマニピュレータ分割表面と構造物分割表面が衝突するかどうかを判定する衝突判定部４０と、この衝突判定部４０でマニピュレータ分割表面と構造物分割表面が衝突すると判定された場合に、衝突を回避するように関節の目標回転角を補正する関節目標回転角補正部４１と、この関節目標回転角補正部４１で補正された目標回転角（若しくは、補正の必要がなかった場合は、関節目標回転角演算部３４で演算された目標回転角）となるように、各駆動部１８のモータ２６ａ，２６ｂへ供給する電圧を制御するモータ電圧制御部４２とを有している。
【００３１】
次に、マニピュレータ１１の手先位置姿勢制御に係わる制御装置１４の制御手順を説明する。図５は、制御装置１４の制御処理内容を表すＰＡＤ図である。
【００３２】
まず、ステップ５０にて、制御装置１４は、図６で示すような設定画面を表示装置１５に表示させ、この設定画面の「操作対象データ読込」ボタン４３ａが操作されると（詳細には、例えばマウスの操作により、画面上のカーソルがボタン位置に合わされた状態でクリックされると）、マニピュレータ１１の設計データを外部の記憶装置（図示せず）から取込んでマニピュレータデータ記憶部３５に保存する。また、設定画面の「周囲構造物読込」ボタン４３ｂが操作されると、構造物の設計データ（好ましくは、マニピュレータ１１の存在する領域に対応して選択された構造物の設計データ）を外部の記憶装置から取込んで構造物データ記憶部３６に保存する。そして、ステップ５１に進み、例えばキーボード等の操作により、設定画面の手元現在位置姿勢入力欄４４にマニピュレータ１１の手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）が入力されると、制御装置１４の手元現在位置姿勢取得部３１は、それらの入力値を入力して保存する。なお、マニピュレータ１１の手元の現在位置（Ｘ０，Ｙ０，Ｚ０）は、上部格子板３及び炉心支持板４におけるガイドパイプ１２の設置位置によって求められる。また、本実施形態では、マニピュレータ１１の手元の現在姿勢は、θＲ０＝０、θＰ０＝０、θＹ０＝０である。
【００３３】
その後、ステップ５２に進み、制御装置１４は、図７で示すような操作画面を表示装置１５に表示させ、マニピュレータ操作処理に移る。このマニピュレータ操作処理では、まず、コントローラ１６の操作の有無にかかわらず、ステップ５３の現在値の取得処理を行う。現在値の取得処理では、ステップ５４の各関節の現在回転角の取得処理、及びステップ５５の手先の現在位置姿勢の演算処理を行う。そして、ステップ５６に進み、コントローラ１６が操作されると、その操作に応じて手先の目標位置姿勢の入力処理を行い、その後、ステップ５７に進み、制御値の演算処理を行う。制御値の演算処理では、ステップ５８の各関節の目標回転角の演算処理、ステップ５９のマニピュレータ分割表面の仮想配置の演算処理、ステップ６０のリスト作成処理、ステップ６１の衝突判定処理、及びステップ６２の関節の目標回転角の補正処理を行う。そして、ステップ６３に進み、目標回転角に基づいてモータ電圧の制御処理を行う。前述した処理の詳細を、以下説明する。
【００３４】
（１）各関節の現在回転角の演算処理（ステップ５４）
各関節の現在回転角の演算処理では、制御装置１４の関節現在回転角取得部３０は、各駆動部１８のエンコーダ２６ａ，２６ｂの検出信号に基づいて各関節の現在回転角（θ１ｒ，θ１ｐ，θ２ｒ，θ２ｐ，θ３ｒ，θ３ｐ，θ４ｒ，θ４ｐ，θ５ｒ，θ５ｐ）を演算する。詳細には、各エンコーダの検出信号（パルス信号）をカウントし、これにセンサ固有のスペックとして予め記憶された所定の回転角度変換係数を乗じて、各関節の現在回転角を演算する。そして、演算した各関節の現在回転角は、図示しないＲＡＭ（Random Access Memory：ランダムアクセスメモリ）等に保存するとともに、表示装置１５の操作画面の関節現在回転角表示欄４５に表示させるようになっている。
【００３５】
（２）手先の現在位置姿勢の演算処理（ステップ５５）
図８は、手先の現在位置姿勢の演算処理の詳細を表すＰＡＤ図である。この手先の現在位置姿勢の演算処理では、制御装置１４の手先現在位置姿勢演算部３２は、まず、ステップ６４にて、手元現在位置姿勢取得部３１で取得されたマニピュレータ１１の手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）を読込み、ステップ６５にて、関節現在回転角取得部３０で取得された関節の現在回転角（θ１ｒ，θ１ｐ，θ２ｒ，θ２ｐ，θ３ｒ，θ３ｐ，θ４ｒ，θ４ｐ，θ５ｒ，θ５ｐ）を読込む。そして、ステップ６６に進み、順運動学演算の同次変換行列演算として、関節毎に予め定義された並進移動行列及び回転移動行列を含む行列式を乗算する。この行列式は、一般的にロボット工学等で用いられているものを使用するため、その詳細な説明を省略する。その後、ステップ６７に進み、ステップ６６で得られた行列（４行４列）の成分を用いて手先の現在位置姿勢（Ｘ，Ｙ，Ｚ，θＲ，θＰ，θＹ）を演算し、演算した手先の現在位置姿勢を図示しないＲＡＭ等に保存するとともに、表示装置１５の操作画面の手先現在位置姿勢表示欄４６に表示させるようになっている。
【００３６】
なお、本実施形態では、制御装置１４の画像構築部（図示せず）は、手元現在位置姿勢取得部３２で取得された手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）を読込み、関節現在回転角演算部３０で取得された各関節の現在回転角（θ１ｒ，θ１ｐ，θ２ｒ，θ２ｐ，θ３ｒ，θ３ｐ，θ４ｒ，θ４ｐ，θ５ｒ，θ５ｐ）を読込み、マニピュレータデータ記憶部３５で記憶されたデータを読込み、マニピュレータ分割表面Ａ１〜Ａｍの現在配置を演算する。詳細には、手元の現在位置姿勢及び各関節の現在回転角に基づいてマニピュレータ分割表面の座標変換処理を実施し、マニピュレータ分割表面の絶対位置座標データを生成し、この絶対位置座標データをＲＡＭ等に保存する。また、マニピュレータ分割表面の絶対位置座標データに基づいてマニピュレータ１１の現在画像を構築する。また、マニピュレータ１１の周囲の構造物のデータを構造物データ記憶部３６から読込み、周囲構造物の画像を構築する。そして、マニピュレータの画像と周囲構造物の画像を合成して表示装置１５の操作画面若しくは設定画面のマニピュレータ画像表示領域４７に表示させるようになっている。これにより、検査員１７がマニピュレータ画像表示領域４７を見て、構造物に対するマニピュレータ１１の現在配置を確認可能としている。
【００３７】
（３）手先の目標位置姿勢の入力処理（ステップ５６）
手先の目標位置姿勢入力処理では、制御装置１４の手先目標位置姿勢入力部３３は、まず、手元現在位置姿勢取得部３１で取得されたマニピュレータ１１の手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）を読込む。そして、コントローラ１６の操作信号を手先の移動回転方向及び移動回転量に換算し、これを手元の現在位置姿勢に加算して目標位置姿勢（Ｘ_ref，Ｙ_ref，Ｚ_ref，θＲ_ref，θＰ_ref，θＹ_ref）を演算する。そして、演算した手先の目標位置姿勢をＲＡＭ等に保存するとともに、表示装置１５の操作画面の手先目標位置姿勢表示欄４８に表示させるようになっている。
【００３８】
（４）各関節の目標回転角の演算処理（ステップ５８）
図９は、各関節の目標回転角の演算処理の詳細を表すＰＡＤ図である。各関節の目標回転角の演算処理では、制御装置１４の関節目標回転角演算部３４は、まず、ステップ６８にて、手先目標位置姿勢入力部３３で入力された手先の目標位置姿勢を読込み、ステップ６９にて、手元現在位置姿勢取得部３１で取得されたマニピュレータ１１の手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）を読込み、ステップ７０にて、関節現在回転角取得部３０で取得された関節の目標回転角（θ１ｒ，θ１ｐ，θ２ｒ，θ２ｐ，θ３ｒ，θ３ｐ，θ４ｒ，θ４ｐ，θ５ｒ，θ５ｐ）を読込む。
【００３９】
そして、ステップ７１に進み、各関節の目標回転角と現在回転角との偏差Δθから手先の目標位置姿勢と現在位置姿勢との偏差ΔＰを演算する際に用いるヤゴビ行列Ｊを算出し（下記の式（１）参照）、ステップ７２に進み、そのヤコビ行列の逆行列Ｊ−１を算出する。ここで、本実施形態のマニピュレータ１１の動作自由度は１０であって、手先の位置姿勢の成分数の６を超えた冗長系であるから、ヤコビ行列Ｊは正方行列ではない。そのため、転置行列ＪＴを用いて疑似逆行列演算を行い、ヤコビ行列の逆行列Ｊ−１を算出する（下記の式（２）参照）。
【００４０】
【数１】
【００４１】
【数２】
【００４２】
【数３】
【００４３】
そして、ステップ７３に進み、手先の目標位置姿勢と現在位置姿勢との偏差ΔＰから各関節の目標回転角と現在回転角との偏差Δθを演算し（上記の式（３）参照）、この偏差Δθをステップ６９で読込んだ各関節の現在回転角に加算して目標回転角（θ１ｒ_ref，θ１ｐ_ref，θ２ｒ_ref，θ２ｐ_ref，θ３ｒ_ref，θ３ｐ_ref，θ４ｒ_ref，θ４ｐ_ref，θ５ｒ_ref，θ５ｐ_ref）を演算する。そして、演算した各関節の目標回転角をＲＡＭ等に保存するとともに、表示装置１５の操作画面の関節目標回転角表示欄４９に表示させるようになっている。
【００４４】
（５）マニピュレータ分割表面の仮想配置の演算処理（ステップ５９）
マニピュレータ分割表面の仮想配置の演算処理では、制御装置１４のマニピュレータ分割表面演算部３７は、手元現在位置姿勢取得部３２で取得された手元の現在位置姿勢（Ｘ０，Ｙ０，Ｚ０，θＲ０，θＰ０，θＹ０）を読込み、関節目標回転角演算部３４で演算された各関節の目標回転角（θ１ｒ_ref，θ１ｐ_ref，θ２ｒ_ref，θ２ｐ_ref，θ３ｒ_ref，θ３ｐ_ref，θ４ｒ_ref，θ４ｐ_ref，θ５ｒ_ref，θ５ｐ_ref）を読込み、マニピュレータデータ記憶部３５で記憶されたデータを読込み、マニピュレータ分割表面Ａ１〜Ａｍの仮想配置を演算する。詳細には、手元の現在位置姿勢及び各関節の目標回転角に基づいてマニピュレータ分割表面の座標変換処理を実施し、マニピュレータ分割表面の絶対位置座標データを生成し、この絶対位置座標データをＲＡＭ等に保存する。
【００４５】
（６）リスト作成処理（ステップ６０）
図１０は、リスト作成処理の詳細を表すＰＡＤ図である。リスト作成処理では、制御装置１４のリスト作成部３８は、まず、ステップ７４にて、マニピュレータデータ記憶部３５で記憶された多数のマニピュレータ分割表面Ａ１〜Ａｍのうちのいずれか１つと構造物データ記憶部３６で記憶された多数の構造物分割表面Ｂ１〜Ｂｎのうちのいずれか１つとの組合せ（図１１参照）を、予め設定された所定の件数（例えば総数の１０％程度）だけランダムに抽出して、抽出リストを作成する。そして、最初のリスト作成処理ではステップ７５（詳細は後述）をとばし、２回目以降のリスト作成処理ではステップ７５を経由してステップ７６に進み、抽出リストに含まれるマニピュレータ分割表面に対し、前述のステップ５９で演算された仮想配置を読込む。また、ステップ７７に進み、抽出リストに含まれる構造物分割表面に対し、構造物データ記憶部３６から絶対位置を読込む。その後、ステップ７８に進み、抽出リストの各組合せにおける仮想配置のマニピュレータ分割表面と構造物分割表面との重心間距離を演算し、ステップ７９に進み、この重心間距離が予め設定された所定の閾値未満であるかどうか判定することによりマニピュレータ分割表面と構造物分割表面が衝突する可能性の高いものであるかどうかを判定する。そして、ステップ８０に進み、衝突する可能性の高いものと判定した組合せからなる候補リストを作成し、その後、ステップ８１に進み、作成した候補リストをリスト記憶部３９に更新して記憶する。
【００４６】
上述したステップ７４〜７８からなるリスト作成処理は、他の処理（衝突判定処理等）のタイミングとは異なり、予め設定された所定の時間間隔（衝突判定処理を行う時間間隔より短い時間間隔）で繰返し行っている。そして、ステップ７５では、前回作成した候補リストの組合せをリスト記憶部３９から読込み、ステップ７４で作成した抽出リストに追加するようになっている。
【００４７】
（７）衝突判定処理（ステップ６１）
衝突判定処理では、制御装置１４の衝突判定部４０は、候補リストに含まれるマニピュレータ分割表面に対し、前述のステップ５９で演算された仮想配置を読込む。また、候補リストに含まれる構造物分割表面に対し、構造物データ記憶部３６から絶対位置を読込む。その後、候補リストの各組合せにおいて、仮想配置のマニピュレータ分割表面と構造物分割表面が衝突するかどうかを、公知の交差判定によって判定するようになっている。
【００４８】
（８）関節の目標回転角の補正処理（ステップ６２）
図１２は、関節の目標回転角の補正処理の詳細を表すＰＡＤ図である。関節の目標回転角の補正処理では、制御装置１４の関節目標回転角補正部４１は、ステップ８２にて、ステップ６１で衝突すると判定された各組合せにおいて、仮想配置のマニピュレータ分割表面と構造部分割表面との位置関係から、仮想配置のマニピュレータ分割表面における頂点のうち最も構造物内側に位置するものを選択し、この頂点と構造物分割表面との距離を演算し、この距離以上の大きさを持ちかつ構造物分割表面の法線ベクトルと同じであって構造物の外側への向きを持つ衝突回避ベクトルを生成する。その後、ステップ８３に進み、マニピュレータ分割表面が複数の関節で区分された複数の部位のいずれに含まれるか判別することにより、ステップ８２で作成した衝突回避ベクトルを分類する。そして、ステップ８３に進み、部位毎に衝突回避ベクトルが１つである場合はそのままとし、複数ある場合は、ステップ８４に進み、衝突回避ベクトルを平均化する。その後、ステップ８５に進み、平均化した衝突回避ベクトルに基づいて関節の目標回転角の補正量を演算し、この補正量をステップ５８で演算された目標回転角に加算する。
【００４９】
以上のように構成された本実施形態においては、制御装置１４は、リスト作成処理にて、マニピュレータ分割表面と構造物分割表面との組合せをランダムに抽出して抽出リストを作成し、その抽出リストの各組合せにおける仮想配置のマニピュレータ分割表面と構造物分割表面との重心間距離を演算し、この重心間距離が所定の閾値未満であるかどうか判定することにより衝突の可能性の高いものであるかどうかを判定し、衝突の可能性の高いものと判定した組合せからなる候補リストを作成する。そして、衝突判定処理にて、候補リストの各組合せにおいて仮想配置のマニピュレータ分割表面と構造物分割表面が衝突するかどうかを判定する。これにより、マニピュレータ１１と構造物との衝突位置や干渉量を把握するためにマニピュレータ分割表面及び構造物分割表面を小さくとり多数に分割する場合であっても、コントローラ１６を操作してから（言い換えれば、マニピュレータ１１の手先の目標位置姿勢を入力してから）マニピュレータ１１が駆動するまでの制御時間の短縮を図ることができる。すなわち、例えばマニピュレータ分割表面と構造物分割表面との総当たりの組合せにおいて衝突するかどうかを判定する制御方法と比べ、衝突判定処理時間の短縮を図ることができ、ひいては全体の制御時間の短縮を図ることができる。また、例えば全てのマニピュレータ分割表面と構造物分割表面との位置関係から衝突の可能性の高い組合せからなる候補リストを作成する制御方法と比べ、候補リスト作成処理時間の短縮を図ることができ、ひいては全体の制御時間の短縮を図ることができる。したがって、本実施形態においては、マニピュレータ１１の手先の目標位置姿勢を指示してから駆動するまでの制御時間の短縮を図りながら、マニピュレータ１１と構造物との衝突回避を図ることができる。
【００５０】
また、本実施形態においては、制御装置１４は、リスト作成処理を所定の時間間隔で繰返し行っており、抽出リストを作成する際に前回作成した候補リストの組合せを加える。これにより、例えば手先の目標位置姿勢や現在位置姿勢及び関節の目標回転角や現在回転角などの変化の有無にかかわらず、時間の経過とともに、候補リストにおける組合せの件数、すなわち衝突判定を行う組合せの件数を増加させることができる。その結果、マニピュレータ１１と構造物との衝突回避性を向上させることができる。
【００５１】
なお、上記一実施形態においては、マニピュレータ分割表面Ａ１〜Ａｍ及び構造物分割表面Ｂ１〜Ｂｎはそれぞれ三角形状をなす場合を例にとって説明したが、これに限られず、四角形等の他の形状をなしてもよいし、複数種類の形状が混在してもよい。このような場合も、上記同様の効果を得ることができる。
【００５２】
また、上記一実施形態においては、表示装置１４は、マニピュレータ１１の現在画像及び周囲構造物の現在画像を構築して表示装置１５の設定画面若しくは操作画面のマニピュレータ画像表示領域４７に表示させる場合を例にとって説明したが、これに限られない。すなわち、例えば原子炉内にマニピュレータ観察用カメラを配置し、このカメラの映像をマニピュレータ画像表示領域４７に表示させてもよい。この場合も、上記同様の効果を得ることができる。
【００５３】
なお、以上においては、本発明の適用対象として、原子炉内構造物の目視検査作業を目的とし、マニピュレータ１１の先端部に目視検査用カメラ１０を搭載した原子炉内検査装置を例にとって説明したが、これに限られない。すなわち、原子炉内構造物に発生した欠陥の長さ及び深さを測定する探傷検査作業を目的とし、マニピュレータ１１の先端部に渦電流センサ又は超音波センサを搭載した原子炉内検査装置に適用してもよい。また、原子炉内構造物の補修作業を目的とし、マニピュレータ１１の先端部に補修器具を搭載した原子炉内補修装置に適用してもよい。また、原子炉以外の狭隘部にてマニピュレータを用いる装置に適用してもよい。その際、原子炉のように水中にてマニピュレータを用いる装置に限られず、気中にてマニピュレータを用いる装置に適用してもよい。これらの場合も、上記同様の効果を得ることができる。
【符号の説明】
【００５４】
１１ マニピュレータ
１４ 制御装置
１６ コントローラ
１８ 駆動部
１９ リンク
２４ａ，２４ｂ モータ
２６ａ，２６ｂ エンコーダ
【特許請求の範囲】
【請求項１】
複数の関節を介し連結された複数のリンクからなるマニピュレータに対し、各関節の回転角を制御することにより手先の位置姿勢を制御する制御装置において、
前記マニピュレータに関するデータとして、各リンクの表面及び各関節の表面を多数に分割したマニピュレータ分割表面の形状及びそれらの相対的な位置関係を記憶する第１記憶手段と、
前記マニピュレータの周囲の構造物に関するデータとして、前記構造物の表面を多数に分割した構造物分割表面の形状及びそれらの絶対位置を記憶する第２記憶手段と、
前記マニピュレータの手元の現在位置姿勢を取得する手元現在位置姿勢取得手段と、
前記マニピュレータの手先の目標位置姿勢を入力する手先目標位置姿勢入力手段と、
前記手元現在位置姿勢取得手段で取得された手元の現在位置姿勢及び前記手先目標位置姿勢入力手段で入力された手先の目標位置姿勢に基づき、各関節の目標回転角を演算する関節目標回転角演算手段と、
前記手元現在位置取得手段で取得された手元の現在位置姿勢、前記関節目標回転角演算手段で演算された各関節の目標回転角、並びに前記第１記憶手段で記憶されたデータに基づき、前記手先の目標位置姿勢に対応する前記多数のマニピュレータ分割表面の仮想配置を演算するマニピュレータ分割表面演算手段と、
前記第１記憶手段で記憶された前記多数のマニピュレータ分割表面のうちのいずれか１つと前記第２記憶手段で記憶された前記多数の構造物分割表面のうちのいずれか１つとの組合せを予め設定された所定の件数だけランダムに抽出して抽出リストを作成し、その抽出リストの各組合せにおける前記仮想配置の前記マニピュレータ分割表面と前記構造物分割表面との重心間距離を演算し、この重心間距離が予め設定された所定の閾値未満であるかどうか判定することにより前記マニピュレータ分割表面と前記構造物分割表面が衝突する可能性の高いものであるかどうかを判定し、衝突する可能性の高いものと判定した組合せからなる候補リストを作成する処理を行うリスト作成手段と、
前記リスト作成手段で作成された候補リストの各組合せにおいて前記仮想配置の前記マニピュレータ分割表面と前記構造物分割表面が衝突するかどうかを判定する衝突判定手段と、
前記衝突判定手段で前記マニピュレータ分割表面と前記構造物分割表面が衝突すると判定された場合に、衝突を回避するように前記関節の目標回転角を補正する関節目標回転角補正手段とを備えたことを特徴とするマニピュレータの制御装置。
【請求項２】
請求項１記載のマニピュレータの制御装置において、
前記リスト作成手段は、
前記処理を予め設定された所定の時間間隔で繰返し行っており、前記抽出リストを作成する際に前回作成した候補リストの組合せを加えることを特徴とするマニピュレータの制御装置。
【請求項３】
請求項１又は２記載のマニピュレータの制御装置において、
前記関節目標回転角補正手段は、
前記衝突判定手段で衝突すると判定された前記仮想配置の前記マニピュレータ分割表面の頂点のうち最も前記構造物の内側に位置するものを選択し、この選択した頂点と前記構造物分割表面との距離以上の大きさを持ちかつ前記構造物分割表面の法線ベクトルと同じであって前記構造物の外側への向きを持つ衝突回避ベクトルを生成する衝突回避ベクトル生成手段と、
前記衝突判定手段で衝突すると判定された前記マニピュレータ分割表面が、前記複数の関節で区分された複数の部位のいずれに含まれるか判別することにより、前記マニピュレータ分割表面に対応する前記衝突回避ベクトルを分類する分類手段と、
前記衝突回避ベクトルを前記部位毎に平均化し、この平均化した衝突回避ベクトルに基づいて前記各関節の目標回転角の補正量を演算する関節目標回転角補正量演算手段とを備えたことを特徴とするマニピュレータの制御装置。
【請求項４】
請求項１〜３のいずれか１項記載のマニピュレータの制御装置において、
前記マニピュレータの各関節の現在回転角を取得する関節現在回転角取得手段と、
前記手元現在位置姿勢取得手段で取得された手元の現在位置姿勢及び前記関節現在回転角取得手段で取得された各関節の現在回転角に基づき、手先の現在位置姿勢を演算する手先現在位置姿勢演算手段とを備え、
前記関節目標回転角演算手段は、
前記手先目標位置姿勢入力手段で入力された手先の目標位置姿勢と前記手先現在位置姿勢演算手段で演算された手先の現在位置姿勢との偏差を演算し、この位置姿勢の偏差に基づき各関節の目標回転角と現在回転角との偏差を演算し、この各関節回転角の偏差を前記関節現在回転角取得手段で取得された各関節の現在回転角に加算して各関節の目標回転角を演算することを特徴とするマニピュレータの制御装置。
【請求項１】
複数の関節を介し連結された複数のリンクからなるマニピュレータに対し、各関節の回転角を制御することにより手先の位置姿勢を制御する制御装置において、
前記マニピュレータに関するデータとして、各リンクの表面及び各関節の表面を多数に分割したマニピュレータ分割表面の形状及びそれらの相対的な位置関係を記憶する第１記憶手段と、
前記マニピュレータの周囲の構造物に関するデータとして、前記構造物の表面を多数に分割した構造物分割表面の形状及びそれらの絶対位置を記憶する第２記憶手段と、
前記マニピュレータの手元の現在位置姿勢を取得する手元現在位置姿勢取得手段と、
前記マニピュレータの手先の目標位置姿勢を入力する手先目標位置姿勢入力手段と、
前記手元現在位置姿勢取得手段で取得された手元の現在位置姿勢及び前記手先目標位置姿勢入力手段で入力された手先の目標位置姿勢に基づき、各関節の目標回転角を演算する関節目標回転角演算手段と、
前記手元現在位置取得手段で取得された手元の現在位置姿勢、前記関節目標回転角演算手段で演算された各関節の目標回転角、並びに前記第１記憶手段で記憶されたデータに基づき、前記手先の目標位置姿勢に対応する前記多数のマニピュレータ分割表面の仮想配置を演算するマニピュレータ分割表面演算手段と、
前記第１記憶手段で記憶された前記多数のマニピュレータ分割表面のうちのいずれか１つと前記第２記憶手段で記憶された前記多数の構造物分割表面のうちのいずれか１つとの組合せを予め設定された所定の件数だけランダムに抽出して抽出リストを作成し、その抽出リストの各組合せにおける前記仮想配置の前記マニピュレータ分割表面と前記構造物分割表面との重心間距離を演算し、この重心間距離が予め設定された所定の閾値未満であるかどうか判定することにより前記マニピュレータ分割表面と前記構造物分割表面が衝突する可能性の高いものであるかどうかを判定し、衝突する可能性の高いものと判定した組合せからなる候補リストを作成する処理を行うリスト作成手段と、
前記リスト作成手段で作成された候補リストの各組合せにおいて前記仮想配置の前記マニピュレータ分割表面と前記構造物分割表面が衝突するかどうかを判定する衝突判定手段と、
前記衝突判定手段で前記マニピュレータ分割表面と前記構造物分割表面が衝突すると判定された場合に、衝突を回避するように前記関節の目標回転角を補正する関節目標回転角補正手段とを備えたことを特徴とするマニピュレータの制御装置。
【請求項２】
請求項１記載のマニピュレータの制御装置において、
前記リスト作成手段は、
前記処理を予め設定された所定の時間間隔で繰返し行っており、前記抽出リストを作成する際に前回作成した候補リストの組合せを加えることを特徴とするマニピュレータの制御装置。
【請求項３】
請求項１又は２記載のマニピュレータの制御装置において、
前記関節目標回転角補正手段は、
前記衝突判定手段で衝突すると判定された前記仮想配置の前記マニピュレータ分割表面の頂点のうち最も前記構造物の内側に位置するものを選択し、この選択した頂点と前記構造物分割表面との距離以上の大きさを持ちかつ前記構造物分割表面の法線ベクトルと同じであって前記構造物の外側への向きを持つ衝突回避ベクトルを生成する衝突回避ベクトル生成手段と、
前記衝突判定手段で衝突すると判定された前記マニピュレータ分割表面が、前記複数の関節で区分された複数の部位のいずれに含まれるか判別することにより、前記マニピュレータ分割表面に対応する前記衝突回避ベクトルを分類する分類手段と、
前記衝突回避ベクトルを前記部位毎に平均化し、この平均化した衝突回避ベクトルに基づいて前記各関節の目標回転角の補正量を演算する関節目標回転角補正量演算手段とを備えたことを特徴とするマニピュレータの制御装置。
【請求項４】
請求項１〜３のいずれか１項記載のマニピュレータの制御装置において、
前記マニピュレータの各関節の現在回転角を取得する関節現在回転角取得手段と、
前記手元現在位置姿勢取得手段で取得された手元の現在位置姿勢及び前記関節現在回転角取得手段で取得された各関節の現在回転角に基づき、手先の現在位置姿勢を演算する手先現在位置姿勢演算手段とを備え、
前記関節目標回転角演算手段は、
前記手先目標位置姿勢入力手段で入力された手先の目標位置姿勢と前記手先現在位置姿勢演算手段で演算された手先の現在位置姿勢との偏差を演算し、この位置姿勢の偏差に基づき各関節の目標回転角と現在回転角との偏差を演算し、この各関節回転角の偏差を前記関節現在回転角取得手段で取得された各関節の現在回転角に加算して各関節の目標回転角を演算することを特徴とするマニピュレータの制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【公開番号】特開２０１３−５２４９８（Ｐ２０１３−５２４９８Ａ）
【公開日】平成２５年３月２１日（２０１３．３．２１）
【国際特許分類】
【出願番号】特願２０１１−１９４１６７（Ｐ２０１１−１９４１６７）
【出願日】平成２３年９月６日（２０１１．９．６）
【出願人】（５０７２５０４２７）日立ＧＥニュークリア・エナジー株式会社 (858)
【Ｆターム（参考）】
【公開日】平成２５年３月２１日（２０１３．３．２１）
【国際特許分類】
【出願日】平成２３年９月６日（２０１１．９．６）
【出願人】（５０７２５０４２７）日立ＧＥニュークリア・エナジー株式会社 (858)
【Ｆターム（参考）】
[ Back to top ]