JP3967394B2 - ワークの組立方法及びその装置 - Google Patents
ワークの組立方法及びその装置 Download PDFInfo
- Publication number
- JP3967394B2 JP3967394B2 JP28358695A JP28358695A JP3967394B2 JP 3967394 B2 JP3967394 B2 JP 3967394B2 JP 28358695 A JP28358695 A JP 28358695A JP 28358695 A JP28358695 A JP 28358695A JP 3967394 B2 JP3967394 B2 JP 3967394B2
- Authority
- JP
- Japan
- Prior art keywords
- workpiece
- work
- segment
- coordinate system
- assembled
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Fluid-Pressure Circuits (AREA)
- Feedback Control In General (AREA)
- Control Of Position Or Direction (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【０００１】
【発明の属する技術分野】
本発明は、油圧シリンダ等の複数のアクチュエータからなるロボットでワークを組み立てるワークの組立方法及びその装置に関するものである。
【０００２】
【従来の技術】
油圧シリンダ等のアクチュエータは、そのサイズに比べて大きな出力を取り出すことができるので、建設等の重量物を扱う場面で広く用いられている。これらの機械では、パワーショベル等の土木機器に見られるような開ループ型の制御や、流量制御弁を用いた速度制御に用いられるものがほとんどで、アクチュエータの発生する力を高精度で制御することはなされていない。
【０００３】
近年、クローズドループ制御を用いて高精度で位置決め制御と速度制御が行える電気・油圧サーボアクチュエータ（ディジタルサーボシリンダ）が開発され、位置制御に用いられるようになってきている。
【０００４】
この電気・油圧サーボアクチュエータは、図９に示すように、油圧シリンダ１０のポート１１，１２に油圧制御弁１３が接続され、その油圧制御弁１３のソレノイド１４に、図示していないサーボドライバから前進及び後進のパルス信号１５を入力することで、油圧ポンプ１６からの圧油を、前進側シリンダ室１７Ｆや後進側シリンダ室１７Ｂに供給してロッド１８をパルス信号１５に応じて進退させるものである。
【０００５】
アクチュエータ１０のロッド１８の位置は、アブソリュート式測長センサ１９とディジタル測長センサ２０とで検出され、このセンサ１９，２０の検出信号を基にしてアクチュエータ１０の位置制御と速度制御を高精度で行えるようになっており、また前進側シリンダ室１７Ｆや後進側シリンダ室１７Ｂの圧力は、それぞれ圧力センサ２１Ｆ，２１Ｂで検出され、その検出値を基に、アクチェータ１０が設定された押圧（引張）力を発揮できるようになっている。
【０００６】
この電気・油圧サーボアクチュエータは、種々の環境に使用され、例えば、射出成形機やアルミダイキャスト成形機、薄膜成形装置、シールド掘進機のセグメント組立装置等で位置決めと力制御に使用されている。また、使用する環境に応じて、単独（シリアルリンク機構）で、或いは複数本同時に駆動（パラレルリンク機構）するように作動される。
【０００７】
【発明が解決しようとする課題】
ところで、従来アクチュエータによる位置制御系と力制御系とは別々に構成され、環境条件に応じてどちらか一方を切り換えて使用していた。
【０００８】
すなわち、位置制御においては、変位センサにより目標位置との変位を制御系にフィードバックして速度指令を出力して目標位置になるように、アクチュエータに圧油を供給する流量弁を制御し、力制御においては、力センサで、環境に作用している力を検出し、その検出値を基にして設定した力が得られるようにアクチュエータを制御するように構成したものであり、制御系が相違するため、それぞれ別個に制御する必要がある。従って、制御を切り換えるには、先ず、位置制御でアクチュエータを目標位置の近くまで作動（微調位置決め作業）した後に、位置制御を停止して、力制御に切り換えアクチュエータで設定の力で押圧（押付作業）するように制御している。
【０００９】
しかしながら、位置制御系と力制御系を切り換える際には、外部環境に応じた切り換え位置の情報が必要であり、そのためのセンシングや制御が別途必要となる。また、これらの情報で切り換えるようにしても外部環境の位置情報が常に得られる保障はなく、例えば速度制御中にワークが外部環境に当たって、それ以上移動できない状況でも位置制御を続けるなどの不具合を生じ、このため、ワークを破損するなどの問題がある。
【００１０】
そこで、本発明者らは、先に位置制御と力制御を切り換えるにおいて、外部環境の位置情報がなくても負荷に応じて切り換えることができるアクチュエータの作動方法及び装置を提案（特願平６−２８５６８４号）した。
【００１１】
この先願の発明の基本的な制御アルゴリズムを図８により説明する。
【００１２】
図８（ａ）、図８（ｂ）において、制御系は、フィードバック制御により仮想的なダンパを構成して位置制御と力制御とが行えるように構成され、１０は、指令速度（ｘd ’）を与えられたアクチュエータ、３０は外力を速度に変換するアドミッタンスがＢ（＝１／ｄ）である仮想ダンパとし、アクチュエータ１０の速度を下式のように遅れなく制御できたとする。
【００１３】
ｘ r' ＝ｘ d' ＋Ｂｆ
この場合、 図８（ａ）では、非拘束状態で位置制御がなされ、ｆ＝０であるため、ｘr'＝ｘd'となり、図８（ｂ）ではワークＷが、外部環境２５に当たり拘束状態となって力制御がなされ、ｘr'＝０であるため、ｆ＝ｘd'／Ｂの力制御がなされる。
【００１４】
上記（１）式による位置・力制御は、多数のアクチュエータからなる多関節ロボットにおいても、目標位置と目標速度をベクトルで求め、このベクトル座標系を、各アクチュエータごとに運動ベクトルに分解して、各アクチュエータのシリンダ長さで定義することで、位置・力制御が可能となる。
【００１５】
しかしながら、先願の発明において、ワークを単純に目標位置まで移動して位置・力制御するには支障はないが、現実にロボットで、多数のワークを、嵌め合せ等の作業で組立てる場合には、単に目標位置に直線的に移動するだけでは作業ができず、上述した基本のアルゴリズムの他に、組立のための組立作業アルゴリズムがない限り実現することが困難である。
【００１６】
そこで、本発明の目的は、上記課題を解決し、位置・力制御を行うにおいてワークを簡単に組み立てるワークの組立方法及びその装置を提供することにある。
【００１７】
【課題を解決するための手段】
上記目的を達成するために、請求項１の発明は、油圧で作動し、測長センサからなる変位センサと圧力センサが設けられた複数のアクチュエータで、多関節ロボットを構成し、そのロボットでワークを、所定の組み付け位置に移動して既設ワークに組み付けるワークの組立方法において、多関節ロボットの原点に対するワークの重心位置を、ロボット基準座標系と、組み付けるワークのワーク座標系と、組み付けの際のワークの作業に適したロール・ピッチ・ヨーの各仮想回転軸をもつ作業座標系で、それぞれ設定すると共に、これらの座標系を多関節ロボットの各アクチュエータの関節座標系で求め、組み付けるワークにローリング・ピッチングを補正する一対のガイドバーを設けると共にその組み付けるワークを、そのワーク座標の原点が粗位置決め目標位置まで、移動するよう各アクチュエータを、ロボット基準座標系を基に位置制御したのち、作業座標系に切り換え、その作業座標系をもとに、かつ上記ガイドバーの既設ワークからの反力により、ローリングとピッチングを調整するアクチュエータに加わる外力をもとにして、ワークをローリングとピッチングの前記仮想回転軸を中心にローリングとピッチング方向を補正すべく移動させて、それぞれローリングとピッチングの組付目標位置に、位置・力制御させて合わせた後、既設ワークと組み付けるワークの反力により、ヨーイングを調整するアクチュエータに加わる外力をもとにして、ヨーイングの前記仮想回転軸を中心に、ヨーイング方向を補正すべく移動させて位置・力制御しながらヨーイングの組付目標位置に合わせて、ワークを最終組付目標位置に組み付けるワークの組立方法である。
【００１８】
請求項２の発明は、ワークが、シールド掘進機でトンネルを構築するセグメントが、その円周方向と軸方向の組み付け面にテーパー面を有するセグメントからなり、既設セグメントにセグメントを組み付ける際、既設セグメントに組み付ける粗位置決め目標位置までセグメントを移動したのち、既設セグメントに対する一対のガイドバーの反力により、ローリング・ピッチングを調整するアクチュエータに加わる外力をもとにして、組み付けるセグメントのロールとピッチ位置を、位置・力制御で合わせたのち、既設セグメントと組み付けるセグメントのテーパ面からの反力により、ヨーイングを調整するアクチュエータに加わる外力をもとにして、組み付けるセグメントのヨーイング位置を、位置・力制御で合わせて、セグメントを最終組付目標位置に組み付ける請求項１記載のワークの組立方法である。
【００１９】
請求項３の発明は、油圧で作動し、測長センサからなる変位センサと圧力センサが設けられた複数のアクチュエータで、多関節ロボットを構成し、そのロボットでワークを、所定の組み付け位置に移動して既設ワークに組み付けるワークの組立装置において、多関節ロボットの原点に対するワークの重心位置を、ロボット基準座標系と、組み付けるワークのワーク座標系と、組み付けの際のワークの作業に適したロール・ピッチ・ヨーの各仮想回転軸をもつ作業座標系で、それぞれ設定すると共に、これらの座標系を多関節ロボットの各アクチュエータの関節座標系で求め、組み付けるワークにローリング・ピッチングを補正する一対のガイドバーを設けると共にその組み付けるワークを、そのワーク座標の原点が粗位置決め目標位置まで、移動するよう各アクチュエータを、ロボット基準座標系を基に位置制御したのち、作業座標系に切り換え、その作業座標系をもとに、かつ上記ガイドバーの既設ワークからの反力により、ローリングとピッチングを調整するアクチュエータに加わる外力をもとにして、ワークをローリングとピッチングの前記仮想回転軸を中心にローリングとピッチング方向を補正すべく移動させて、それぞれローリングとピッチングの組付目標位置に、位置・力制御させて合わせた後、既設ワークと組み付けるワークの反力により、ヨーイングを調整するアクチュエータに加わる外力をもとにして、ヨーイングの前記仮想回転軸を中心に、ヨーイング方向を補正すべく移動させて、位置・力制御しながらヨーイングの組付目標位置に合わせて、ワークを最終組付目標位置に組み付けるための制御手段を備えたワークの組立装置である。
【００２０】
上記の構成によれば、ワークの組み付け状態に着目し、ワークを組み付ける際の作業座標系をロボットの各アクチュエータの関節座標で定義しておくことで、組み付けの際に、ワークをローリング・ピッチング・ヨーイング方向に簡単に力制御でき、組み付け位置の厳密な位置情報がなくても組み付けが可能となる。
【００２１】
【発明の実施の形態】
以下、本発明の好適実施の形態を添付図面に基づいて詳述する。
【００２２】
先ず本発明を説明する前に、先願の発明における位置・力制御を行うための装置の概略構成と制御ブロック図を、図１０，図１１で簡単に説明する。
【００２３】
図１０、図１１において、アクチュエータ１０の構成は図９で説明した通りである。
【００２４】
このアクチュエータ１０には、アブソリュート式測長センサとディジタル測長センサからなる変位センサ２９が設けられると共に圧力センサ２１が設けられる。
【００２５】
変位センサ２９は、アクチュエータ１０のロッド１８の進退位置を検出し、これを位置信号３５として速度指令生成部４０に送ると共に速度サーボ部４１にフィードバック信号として送る。また圧力センサ２１の検出圧力は速度指令生成部４０に入力される。
【００２６】
速度指令生成部４０は、変位センサ２９からの位置信号３５を基に軌道生成部４２で目標速度（ｘr ’）を生成し、同時に、圧力センサ２１の検出圧力（−ｆ）、または、外部環境２５から反力Ｆとして伝わる外力（−ｆ）を基にアドミッタンス設定部４３で、その外力ｆをアドミッタンスＢで速度信号（Ｂｆ）に変換すると共に、これらを加算器４４にて、アクチュエータ速度（ｘd ’）を、ｘr ’＝ｘ d ’−Ｂｆとなるようにして速度サーボ部４１に出力する。
【００２７】
この設定部４３でのアドミッタンスＢは、そのアクチュエータの最大押し付け力の逆数と最大速度の乗算値を基に設定され、圧力センサ２１の検出圧に応じて速度信号（Ｂｆ）を作り出すようになっている。
【００２８】
速度指令生成部４０で生成されたアクチュエータ速度（ｘd ’）は、速度サーボ部４１に入力されると、先ず、速度サーボ部４１は、アクチュエータ速度（ｘd ’）に基づいて、フィードフォワード速度信号ｖr を作り、これを加算器４５を介して速度指令値４６をサーボアンプ４７に出力し、サーボアンプ４７が制御電流４８を作ってこれをアクチュエータ１０に出力するようになっている。
【００２９】
またアクチュエータ速度（ｘd ’）は、積分器４９で、位置信号ｘr にされて比較器５０に入力され、そこで変位センサ２９からフィードバックされた位置（ｘ）信号３５と比較されて位置補償制御部５１に入力され、その位置補償制御部５１にて速度信号にされ、加算器４５にてフィードフォワード速度信号ｖr と加算されてアクチュエータ１０を作動する速度指令４６が作られる。
【００３０】
以上において、速度指令生成部４０では、アクチュエータ速度（ｘd ’）が、
ｘd ’＝ｘr ’＋Ｂｆ
となるように制御する。
【００３１】
このようにアクチュエータ速度ｘd ’を作りだしこれを速度サーボ部４１及びサーボアンプ４７を介してアクチュエータ１０を制御することで位置制御と力制御の双方を自動的に切り換えて制御することができる。
【００３２】
この図１０，図１１のアクチュエータ１０は、１軸の場合であるが、複数のアクチュエータ１０で多関節ロボットを構成する場合でも、目標方向と目標速度を、ヤコビアンを用いて各アクチュエータの運動ベクトルに分解すれば各アクチュエータごとに位置・力制御が行える。
【００３３】
さて、本発明のワーク組立方法及びその装置を説明する。
【００３４】
今、多関節ロボット６０として、シールド掘進機のセグメント組立装置のロボットを例に図４〜図７により説明する。
【００３５】
シールド掘進機は、シールドフレーム前面のカッタで地山を掘進し、その後方で、セグメント組立装置で、セグメントを円周方向に組み立ててトンネルを構築していくもので、そのセグメントとして最近、円弧状のセグメントからハニカム状のセグメントなど円周方向と軸方向の組み付け面にテーパ面を有するセグメントを組立ててトンネルを構築することがなされている。
【００３６】
図４（ａ）において、６１はエレクタリングで、ラック・ピニオン６２により旋回微調整アクチュエータ６３で、ロボット原点Ｏ0 を中心に回転され、そのエレクタリング６１に、支持部材６４が、一対の径方向アクチュエータ６５ｌ，ｒで、ロボット原点Ｏ0 に対して径方向に移動され、その支持部材６４に図４（ｂ）に示すように一対のヨーイング調整用アクチュエータ６６ｌ，ｒを介して摺動フレーム６７が、ヨーイング自在に支持され、その摺動フレーム６７に３本のローリング・ピッチング調整用アクチュエータ６８で、セグメント７０が支持される。
【００３７】
また摺動フレーム６７には坑口側に既設セグメントの内周面と接してセグメント７０のピッチング・ローリングを補正するためのガイドバー６９が設けられている。
【００３８】
このセグメント組立装置のロボットで、既設セグメントにセグメントを組み付ける手順を図６により説明する。
【００３９】
先ず、図６（ａ）に示すように既設セグメント７０ｓの組み付け位置に位置するようにセグメント７０が径方向外方に移動される。この際、セグメント７０は、ピッチング方向の姿勢が既設セグメント７０ｓの内周面と一致しない場合、ガイドバー６９の前後の接触子７１ｆ，ｂのいずれかが、図６（ｂ）、（ｃ）に示すように当たり、仮想原点Ｏβを通る軸中心にピッチし、これら接触子７１ｆ，ｂが図６（d ）に示すように均等に当たるようにピッチング方向の姿勢が調整される。
【００４０】
また、ローリング方向についても、図６（ｅ）に示すようにセグメント７０が径方向外方に移動される際に、図６（ｆ）に示すようにガイドバー６９の接触子７１ｌ，ｒのいずれかが当たり、仮想原点Ｏγを通る軸中心にロールし、これら接触子７１ｌ，ｒが図６（ｇ）に示すように均等に当たるようにローリング方向の姿勢が調整される。
【００４１】
このように、ピッチング方向とローリング方向の姿勢が調整されたセグメント７０は、図７（ａ）〜（ｃ）に示すように既設セグメント７０ｓの所定位置に組み付けるべく仮想原点Ｏαを通る軸中心にヨーし、ヨーイング方向を調整しながら所定の位置に組み付けられることとなる。
【００４２】
本発明においての組み付けの基本アルゴリズムは、ワークであるセグメント７０を、所定の組み付け位置に組み付ける際、図５（ａ），（ｂ）に示すように、先ずセグメント７０を組み付け目標位置（ｒd0，θd0，ｚd0）に位置制御で移動し、次に最終組付目標位置（ｒdc，θdc，ｚdc）まで、上述した仮想原点Ｏ（α，β，γ）を中心に、ピッチング・ローリング・ヨーイング方向を補正すべく位置・力制御しながら移動してセグメント７０を最終組付目標位置に組み付けることにある。この際、セグメント７０と共に移動する一対のガイドバー６９が、既設セグメント７０ｓに当たり、その反力をもとに、仮想原点Ｏ（α，β）を中心にピッチング・ローリング方向を補正すべく移動させて、ピッチング・ローリングの組付目標位置（ｒ dc ，θ dc ）に位置合わせを行ったのち、既設セグメント７０ｓとセグメント７０の反力をもとに、仮想原点Ｏ（γ）を中心にヨーイング方向を補正すべく位置・力制御しながら移動してヨーイングの組付目標位置（ｚ dc ）に合わせてセグメント７０を最終組付目標位置に（ｒ dc ，θ dc ，ｚ dc ）に組み付ける。
【００４３】
このセグメントを組み付けるための組み付けアルゴリズムを説明する。
【００４４】
今、上述したセグメント組立ロボットのモデル化して簡便に表すと図２に示したようになり、各アクチュエータ６３，６５，６６，６８の各関節座標系で表せる。
【００４５】
ここで、図２に示す記号と、以下で説明するアルゴリズムで使用する記号を以下に纏めて示す。計算で用いる単位系は、ＳＩ単位系を使用する。
【００４６】
(1) 座標系に関する記号
ξ ：セグメント位置座標ベクトル［ｍ］（３×１）
セグメント座標原点のロボット基準座標系で表した位置ベクトル
Ｔ ：セグメント座標姿勢行列（３×３）
セグメント座標原点のロボット基準座標系で表した回転行列
ｗ ：セグメント位置姿勢座標［ｍ／ｒａｄ］（７×１）
ｗ＝（ｘ，ｒ，θ，α，β，γ，ｈ）T （ｈ；把持部長さ）
セグメント座標系を作業座標系で表した位置／姿勢ベクトル
Ｔwi：作業座標姿勢行列（３×３）［ｉ；０〜３］
作業座標回転（０；θ回転、１；ヨー、２；ピッチ、３；ロール）
による姿勢を表す回転行列
ξ0 ：セグメント位置座標ベクトル［ｍ］（３×１）
作業座標回転０での把持位置ベクトル
ξ1 ：回転補正位置［ｍ］（３×１） ［ｉ；１〜３］
作業座標回転（１；ヨー、２；ピッチ、３；ロール）での把持位置
補正用ベクトル
ηi ：関節ｉ座標原点位置ベクトル［ｍ］（３×１） ［ｉ；１〜７］
関節ｉ座標原点を関節ｉ−ｌ座標系で表した位置ベクトル
ただし、ここで定義した座標系では以下が成立する。
【００４７】
η1 ＝η4 ＝η6 ＝Ｏ（０ベクトル）
Ａi ：関節ｉ座標姿勢行列（３×３） ［ｉ；１〜７］
関節ｉ座標姿勢を関節ｉ−ｌ座標系で表した回転行列
ただし、ここで定義した座標系では以下が成立する。
【００４８】
Ａ2＝Ａ3＝Ａ7＝Ｉ（単位行列）
η8 ：セグメント位置ベクトル［ｍ］（３×１）
セグメント座標原点を関節７座標系で表した位置ベクトル
(2)ロボット機構変数に関する記号
ｌ ：アクチュエータ変位［ｍ］（８成分）
ｌl ：旋回軸長さ、 ｌ2r：伸縮右長さ、 ｌ2l：伸縮左長さ
ｌ3r：摺動右長さ、 ｌ3l：摺動左長さ
ｌ5r：微長右長さ、 ｌ5l：微長左長さ、 ｌ5m：微長中長さ
ｑ ：関節変数ベクトル（７×１）
ｆk ：アクチュエータ変位−関節変数関係式ベクトル［ｉ；１〜４］
ｑ1 ＝ｆk1（ｌ1）
ｑ2 ＝ｆk2（ψ1）＝（ｌ2r ＋ｌ2l）／２，
ψ1＝（ｌ2r，ｌ2l）T
φ2 ＝ｆk3（ψ2）,φ2 ＝（ｑ3 ，ｑ4）T ,
ψ2＝（ｌ3r，ｌ3l）T
φ3 ＝ｆk4（ψ3）,φ3 ＝（ｑ5,ｑ6,ｑ7）T,
ψ3＝（ｌ5r,ｌ 5 ｌ,ｌ5m）T
(3)ロボット機構定数２関する記号（スカラ量）
Ｌ01 ：ヨー軸オフセット量［ｍ］
Ｌ02 ：ピッチ軸オフセット量［ｍ］
ｒ0 ：旋回微調整軸半径［ｍ］
Ｌa ：摺動左軸オフセット［ｍ］
Ｌb ：微調軸ｙオフセット［ｍ］
Ｌc ：微調軸ｚ軸オフセット［ｍ］
Ｌd ：微調軸摺動右軸オフセット［ｍ］
Ｌe ：セグメントオフセット［ｍ］
Θ2 ：摺動ステージｙ回転オフセット角［ｒａｄ］
Ω2 ：摺動ステージｘ回転オフセット角［ｒａｄ］
Θ3 ：ヨーイング軸オフセット角［ｒａｄ］
Θ4 ：摺動軸オフセット角［ｒａｄ］
さて、図１において、ロボットの原点Ｏ0 に対するロボット基準座標系を（Ｘ0 ，Ｙ0 ，Ｚ0 ）、またロボットの原点Ｏ0 に対する位置をｒ，θの円筒座標とｘ方向の座標系をとし、セグメント７０の重心位置を原点Ｏｓとし、その座標系を（Ｘｓ，Ｙｓ，Ｚｓ）とすると、仮想原点Ｏ(αβγ)は、セグメント原点Ｏｓに対してＸｓ方向でＬ01オフセットし、Ｙｓ方向にＬ02オフセットしている。
【００４９】
この仮想原点Ｏ( αβγ) におけるピッチβはＬ02オフセットしたＹβ軸廻りに、ロールγはＸγ（＝Ｘｓ）軸廻りに、ヨーαは、Ｘｓ方向にＬ01オフセットしたＺα軸廻りに回転することになる。
【００５０】
そこで、基準座標系からみたセグメント座標系の位置ξ，姿勢Ｔを、仮想原点Ｏ(αβγ)における各軸廻り、すなわちロール・ピッチ・ヨーの仮想回転軸をもつ作業座標系の位置ベクトル成分で表現する。
【００５１】
Ａ．作業座標系と基準座標系の関係式導出
(1) 各回転のみを行った場合の姿勢と位置補正量
ａ．ヨーイングＴw1は、Ｚα軸廻りの回転行列として数１で表現される。
【００５２】
【数１】
【００５３】
ヨーイング軸Ｚαは、セグメント原点とＬ1 だけオフセットしているので、位置ξ1 は、数２の量だけずれる。
【００５４】
【数２】
【００５５】
ｂ．ピッチングＴw2はＹβ軸廻りの回転行列として、数３で又、位置ξ2 は、セグメント原点とＬ2 だけオフセットしているので数４で表せる。
【００５６】
【数３】
【００５７】
【数４】
【００５８】
ｃ．ローリングＴw3は、Ｘα（＝Ｘｓ）軸廻りの回転行列として数５で表され、位置ξ3 は、オフセットが０であるため数６で表される。
【００５９】
【数５】
【００６０】
【数６】
【００６１】
(2) ｒ，θ，ｘによる位置Ｔw0と姿勢ξ0 は、数７，数８で表される。
【００６２】
【数７】
【００６３】
【数８】
【００６４】
回転順序を、ヨー，ピッチ，ロールの順で定義すると、Ｔ，ξは、下式で求められる。
【００６５】
Ｔ＝Ｔw0・Ｔw1・Ｔw2・Ｔw3 …(1)
ξ＝ξ0 ＋Ｔw0ξ1 ＋Ｔw0Ｔw1ξ2 ＋Ｔw0Ｔw1Ｔw3ξ3 …(2)
Ｂ．関節変数と基準座標系の関係式導出
次に、基準座標系からみた、セグメント座標系の位置ξ，姿勢Ｔを各アクチュエータの関節変数ｑi を用いて表現する。
【００６６】
(1) 姿勢Ｔ
各関節ごとに固定した座標系間の回転行列を定義しているので、その積で、下式のように求めることができる。
【００６７】
Ｔ＝Ａ1 ・Ａ2 ・…・Ａ7 …(3)
このＡ1 〜Ａ7 の成分は、図２で説明した関節変数ｑ1 〜ｑ7 と機構に関する定数で容易に表現できる。
【００６８】
(2) 位置ξ
各関節ごとの座標原点間のベクトルの和で下式で求めることができる。
【００６９】
ξ＝Ａ1 η1 ＋Ａ1 Ａ2 η2 ＋Ａ1 Ａ2 Ａ3 η3 ＋…
＋Ａ1 Ａ2 Ａ3 …Ａ7 （η7 ＋η8 ） …(4)
η1 〜η8 の成分は、図２で説明したように関節変数ｑと機構に関する定数で表現できる。
【００７０】
Ｃ．ヤコビ行列の導出
上記(1),(3) 式と(2),(4) 式は同じ値を示しているので、成分ごとに符号を取ると以下のようにまとめることができる。
【００７１】
ｗ＝ｆk （ｑ） … (5)
(5) 式の両辺を偏微分することによりヤコビ行列が求められる。
【００７２】
ｗ’＝Ｊｑ’ … (6)
Ｊ ＝ ｄｆk ／ｄｑ … (7)
ｑと関節駆動アクチュエータは、機構学的な関係から以下のように求められる。
【００７３】
ｑ1 ＝ ｆk1（ｌ1 ） … (8)
ｑ2 ＝ ｆk2（ψ1 ） … (9)
φ2 ＝ ｆk3（ψ2 ） …(10)
φ3 ＝ ｆk4（ψ3 ） …(11)
(8) 〜(11)式をそれぞれ偏微分すると
ｑ1 ’＝ Ｊk1・ｌ1 …(12)
ｑ2 ’＝ Ｊk2・ψ1 …(13)
φ2 ’＝ Ｊk3・ψ2 …(14)
φ3 ’＝ Ｊk4・ψ3 …(15)
また、ｑは数９なので、数９を偏微分すると数１０となる。
【００７４】
【数９】
【００７５】
【数１０】
【００７６】
次にアクチュエータ出力と作業座標の問を表すヤコビ行列は、下式で求められる。
【００７７】
ｗ’＝Ｊ・Ｊｑ・ｌ’ …(16)
よってヤコビ行列を用いることで、作業座標系での力制御を実現できるようになる。
【００７８】
図３は、本発明のワーク（セグメント）と組み立てるための位置／力のハイブリッド制御系のブロック図を示す。
【００７９】
この図３の制御系を説明する前に力制御アルゴリズムに関する記号を説明する。
【００８０】
ｐ ：アクチュエータ発生圧力［Ｐａ］（８×２成分）
ｆl ：アクチュエータ発生力ベクトル［Ｎ］（７×１）
ｆ ：セグメント発生力［Ｎ／Ｎｍ］（７×１）
ｆ＝（ｆx ，ｆr ，ｍθ，ｍα，ｍβ，ｍγ，ｆg ）T
（ロボット作業座標系ベクトル／ｆg ；把持力）
ｖr ：アクチュエータ指令速度［ｍ／ｓ］（８×１）
ｗd ：力フィードバック速度［（ｍ／ｓ）／（ｒａｄ／ｓ）］（７×１）
ｗr ：力制御目標速度［（ｍ／ｓ）／（ｒａｄ／ｓ）］（７×１）
ｌ’r ：アクチュエータ目標速度［ｍ／ｓ］（８×１）
ｖd ：アクチュエータ指令速度［ｍ／ｓ］（８×１）
Ｊ ：ヤコビ行列（８×１）
Ｊ＝ｄｆk ／ｄｑ
ｗ＝ｆk （ｑ）；順運動学関係式ベクトル（７×１）
（ｗ’＝Ｊｑ’）
Ｊ-1 ：ヤコビ行列逆行列（７×７）
ＪT ：ヤコビ行列転置逆行列（７×７）
Ｂ ：アドミッタンス行列［(m/s)/Ｎ／(rad/s)/Ｎｍ］（７×７; 対角行列）
Ｓ ：力制御選択行列（７×７; 対角行列）
Ｉ ：単位行列（７×７）
Ｊi ：関節変数行列
Ｊ1 ＝ｄｆk1／ｄｌ1 （スカラ関数）
Ｊ2 ＝ｄｆk2／ｄψ2 （２×１行列）
Ｊ3 ＝ｄｆk3／ｄψ2 （２×２行列）
Ｊ4 ＝ｄｆk1／ｄψ3 （３×３行列）
さて今、理想的な力のセンシングができればダンピング制御を用いた力制御で位置制御も可能であるが、現実にはセンサのノイズの影響があり、厳密な位置決め制御は実現できない。
【００８１】
組立作業において、接触のない方向に対しては力制御の必要がないことから、位置制御／力制御を選択できる制御系を構成する。
【００８２】
そこで、力制御選択行列Ｓを以下のように定義し、下式に示す制御アルゴリズムで実現できる。
【００８３】
ｗr ’＝Ｓ（ｗd ’＋Ｂｆ）＋（Ｉ−Ｓ）ｗd ’ …(17)
Ｓ＝diag［s1，s2，・・，s7］
si＝１：ｉ軸力制御選択
si＝０：ｉ軸位置制御選択
Ｉ：単位行列
上記(17)式によって得られた力制御目標速度を(16)式で各アクチュエータの指令速度に変換し、サーボ系を構成することによって力制御が実現できる。
【００８４】
このようにして構成された制御ブロック図を図３で説明する。
【００８５】
図３において、速度指令生成部で生成される目標速度ｗd'が軌道計画生成部７５で生成し、これを位置制御選択行列（Ｉ−Ｓ）７６と加算器７７を介して力制御行列（Ｓ）７８に出力する。ここでＩは単位行列、Ｓは選択行列で、それぞれその要素が１または０の対角行列であり、１は、それに対応する座標でアドミッタンス行列（Ｂ）７９を基に力制御をすることを意味し、０は、位置制御を行うことを意味している。
【００８６】
位置制御選択行列（Ｉ−Ｓ）７６と力制御行列（Ｓ）７８の出力は、加算器８０にて加算されて力制御目標速度ｗｒとされ、ヤコビ行列逆行列８１にてそれぞれアクチュエータごとの運動ベクトルに分解される。
【００８７】
制御の開始時には、反力Ｆ＝０であり、位置行列選択７６で生成される座標がヤコビ行列逆行列８１より、速度制御補償部８２にて各アクチュエータ１０の速度信号ｖr が生成され、加算器８３を介してフィードフォワード速度信号としてアクチュエータ１０に出力される。一方ヤコビ行列逆行列８１にて分解された運動ベクトルは積分器８４で、各アクチュエータのシリンダ位置信号８５にされて比較器８６に入力され、そこで各アクチュエータ１０からフィードバックされたシリンダ変位信号（Ｉ）８７と比較されて位置補償制御部８８に入力され、その位置補償制御部８８にて速度信号にされ、加算器８３にてフィードフォワード速度信号ｖr と加算されて各アクチュエータ１０を作動する速度指令Ｖｄが作られ、その速度指令Ｖｄに基づいてアクチュエータ１０からなるセグメントロボットが、目標軌道に沿った速度ｗr で移動して外部環境２５に接触するまで位置制御する。
【００８８】
次に外部環境２５にワーク２４が当たり移動が停止して、速度ｘr ’が０となると、各アクチュエータ１０の圧力センサからシリンダ発生力（Ｆｌ）９０が、ヤコビ行列転置逆行列９１に送られて外力Ｆに変換され、アドミッタンス行列７９により外力Ｆが速度ベクトル（ＢＦ）にされ、力制御選択行列７８により力制御に切り換えられ、セグメントに設定の力が加えられることなる。
【００８９】
この制御において、上述したようにセグメントを粗位置決め目標位置に移動すると、既設セグメントなどの外部環境２５より、位置制御から力制御に切り換える際のピッチング・ローリング・ヨーイング方向の調整は、図６に示すように、位置制御のときのアクチュエータ速度ｖ1rが、力制御のときのセグメント発生力ｆr でピッチング方向の反力ｆ1rf ，ｆ1rb とローリング方向の反力ｆ1rr ，ｆ1rl が均等になるためのセグメント発生力ｆ1rとなるように力制御すれば良く、また同様にヨーイングに関しても、図７に示したように、速度ｖ2zで位置制御し、セグメント発生力ｆz で力制御したとき、ヨーイング方向の反力ｆ2z1 ，ｆ2z2 が釣合い、反力ｆ2zo でｆ2zの発生力となったときに、正しくセグメント７０が組み付けられたとして制御を完了する。
【００９０】
このように本発明においては、粗位置決め目標位置から、位置・力制御により最終組付目標位置までピッチ・ロール・ヨーを調整しながらセグメントを正確に組み付けることが可能となる。
【００９１】
上述の実施の形態においては、組立てるワークとしてシールド掘進機のトンネル構築用のセグメントの例で説明したが、セグメントに限らず種々のワークの組立に適用できる。また、組み付けの際にロール・ピッチ・ヨーを行い、ヨーイングを最後に行う例で説明したが、これらはワークの組み付け方や形状によっていずれの順序で行っても良いし、いずれか１つか２つでも良い。
【００９２】
【発明の効果】
以上要するに本発明によれば、変位センサと圧力センサが設けられた複数のアクチュエータで、多関節ロボットを構成し、多関節ロボットの原点に対するワークの重心位置を、ロボット基準座標系と、組み付けるワークのワーク座標系と、組み付けの際のワークの作業に適したロール・ピッチ・ヨーの各仮想回転軸をもつ作業座標系で、それぞれ設定すると共に、これら座標系を多関節ロボットの各アクチュエータの関節座標で求め、組み付けるワークに一対のガイドバーを設け、ワークを組み付ける際に、ロボット基準座標系とワーク座標系で、粗置決め目標位置まで移動したときに、作業座標系に切り換え、ガイドバーから各アクチュエータに加わる外力をもとに、位置・力制御でワークのロール・ピッチの組付目標位置に調整した後、既設ワークからの反力をもとに、位置・力制御でワークのヨーの組付目標位置を調整することで、組み付け位置の厳密な位置情報がなくても最終組付目標にワークを組み付けることが可能となる。
【図面の簡単な説明】
【図１】本発明において、作業座標系の設定を説明する概略図である。
【図２】本発明におけるロボットの関節座標系を説明する概略図である。
【図３】本発明における位置・力制御の制御ブロック図を示す図である。
【図４】本発明において、ロボットとしてのセグメント組立装置を示す概略図である。
【図５】本発明において、粗位置決めと組み付け目標位置の概念図を示す図である。
【図６】本発明において、ピッチングとローリング補正を説明する概略図である。
【図７】本発明において、ヨーイング・旋回位置補正を説明する概略図である。
【図８】先願の発明におけるアクチュエータ位置・力制御を説明する概念図である。
【図９】先願及び本発明に用いるアクチュエータの詳細を示す図である。
【図１０】先願の発明におけるシリアルリンクの制御装置を説明する概略図である。
【図１１】先願の発明におけるアクチュエータの制御ブロック図を示す図である。
【符号の説明】
１０ アクチュエータ
７０ セグメント（ワーク）
【発明の属する技術分野】
本発明は、油圧シリンダ等の複数のアクチュエータからなるロボットでワークを組み立てるワークの組立方法及びその装置に関するものである。
【０００２】
【従来の技術】
油圧シリンダ等のアクチュエータは、そのサイズに比べて大きな出力を取り出すことができるので、建設等の重量物を扱う場面で広く用いられている。これらの機械では、パワーショベル等の土木機器に見られるような開ループ型の制御や、流量制御弁を用いた速度制御に用いられるものがほとんどで、アクチュエータの発生する力を高精度で制御することはなされていない。
【０００３】
近年、クローズドループ制御を用いて高精度で位置決め制御と速度制御が行える電気・油圧サーボアクチュエータ（ディジタルサーボシリンダ）が開発され、位置制御に用いられるようになってきている。
【０００４】
この電気・油圧サーボアクチュエータは、図９に示すように、油圧シリンダ１０のポート１１，１２に油圧制御弁１３が接続され、その油圧制御弁１３のソレノイド１４に、図示していないサーボドライバから前進及び後進のパルス信号１５を入力することで、油圧ポンプ１６からの圧油を、前進側シリンダ室１７Ｆや後進側シリンダ室１７Ｂに供給してロッド１８をパルス信号１５に応じて進退させるものである。
【０００５】
アクチュエータ１０のロッド１８の位置は、アブソリュート式測長センサ１９とディジタル測長センサ２０とで検出され、このセンサ１９，２０の検出信号を基にしてアクチュエータ１０の位置制御と速度制御を高精度で行えるようになっており、また前進側シリンダ室１７Ｆや後進側シリンダ室１７Ｂの圧力は、それぞれ圧力センサ２１Ｆ，２１Ｂで検出され、その検出値を基に、アクチェータ１０が設定された押圧（引張）力を発揮できるようになっている。
【０００６】
この電気・油圧サーボアクチュエータは、種々の環境に使用され、例えば、射出成形機やアルミダイキャスト成形機、薄膜成形装置、シールド掘進機のセグメント組立装置等で位置決めと力制御に使用されている。また、使用する環境に応じて、単独（シリアルリンク機構）で、或いは複数本同時に駆動（パラレルリンク機構）するように作動される。
【０００７】
【発明が解決しようとする課題】
ところで、従来アクチュエータによる位置制御系と力制御系とは別々に構成され、環境条件に応じてどちらか一方を切り換えて使用していた。
【０００８】
すなわち、位置制御においては、変位センサにより目標位置との変位を制御系にフィードバックして速度指令を出力して目標位置になるように、アクチュエータに圧油を供給する流量弁を制御し、力制御においては、力センサで、環境に作用している力を検出し、その検出値を基にして設定した力が得られるようにアクチュエータを制御するように構成したものであり、制御系が相違するため、それぞれ別個に制御する必要がある。従って、制御を切り換えるには、先ず、位置制御でアクチュエータを目標位置の近くまで作動（微調位置決め作業）した後に、位置制御を停止して、力制御に切り換えアクチュエータで設定の力で押圧（押付作業）するように制御している。
【０００９】
しかしながら、位置制御系と力制御系を切り換える際には、外部環境に応じた切り換え位置の情報が必要であり、そのためのセンシングや制御が別途必要となる。また、これらの情報で切り換えるようにしても外部環境の位置情報が常に得られる保障はなく、例えば速度制御中にワークが外部環境に当たって、それ以上移動できない状況でも位置制御を続けるなどの不具合を生じ、このため、ワークを破損するなどの問題がある。
【００１０】
そこで、本発明者らは、先に位置制御と力制御を切り換えるにおいて、外部環境の位置情報がなくても負荷に応じて切り換えることができるアクチュエータの作動方法及び装置を提案（特願平６−２８５６８４号）した。
【００１１】
この先願の発明の基本的な制御アルゴリズムを図８により説明する。
【００１２】
図８（ａ）、図８（ｂ）において、制御系は、フィードバック制御により仮想的なダンパを構成して位置制御と力制御とが行えるように構成され、１０は、指令速度（ｘd ’）を与えられたアクチュエータ、３０は外力を速度に変換するアドミッタンスがＢ（＝１／ｄ）である仮想ダンパとし、アクチュエータ１０の速度を下式のように遅れなく制御できたとする。
【００１３】
ｘ r' ＝ｘ d' ＋Ｂｆ
この場合、 図８（ａ）では、非拘束状態で位置制御がなされ、ｆ＝０であるため、ｘr'＝ｘd'となり、図８（ｂ）ではワークＷが、外部環境２５に当たり拘束状態となって力制御がなされ、ｘr'＝０であるため、ｆ＝ｘd'／Ｂの力制御がなされる。
【００１４】
上記（１）式による位置・力制御は、多数のアクチュエータからなる多関節ロボットにおいても、目標位置と目標速度をベクトルで求め、このベクトル座標系を、各アクチュエータごとに運動ベクトルに分解して、各アクチュエータのシリンダ長さで定義することで、位置・力制御が可能となる。
【００１５】
しかしながら、先願の発明において、ワークを単純に目標位置まで移動して位置・力制御するには支障はないが、現実にロボットで、多数のワークを、嵌め合せ等の作業で組立てる場合には、単に目標位置に直線的に移動するだけでは作業ができず、上述した基本のアルゴリズムの他に、組立のための組立作業アルゴリズムがない限り実現することが困難である。
【００１６】
そこで、本発明の目的は、上記課題を解決し、位置・力制御を行うにおいてワークを簡単に組み立てるワークの組立方法及びその装置を提供することにある。
【００１７】
【課題を解決するための手段】
上記目的を達成するために、請求項１の発明は、油圧で作動し、測長センサからなる変位センサと圧力センサが設けられた複数のアクチュエータで、多関節ロボットを構成し、そのロボットでワークを、所定の組み付け位置に移動して既設ワークに組み付けるワークの組立方法において、多関節ロボットの原点に対するワークの重心位置を、ロボット基準座標系と、組み付けるワークのワーク座標系と、組み付けの際のワークの作業に適したロール・ピッチ・ヨーの各仮想回転軸をもつ作業座標系で、それぞれ設定すると共に、これらの座標系を多関節ロボットの各アクチュエータの関節座標系で求め、組み付けるワークにローリング・ピッチングを補正する一対のガイドバーを設けると共にその組み付けるワークを、そのワーク座標の原点が粗位置決め目標位置まで、移動するよう各アクチュエータを、ロボット基準座標系を基に位置制御したのち、作業座標系に切り換え、その作業座標系をもとに、かつ上記ガイドバーの既設ワークからの反力により、ローリングとピッチングを調整するアクチュエータに加わる外力をもとにして、ワークをローリングとピッチングの前記仮想回転軸を中心にローリングとピッチング方向を補正すべく移動させて、それぞれローリングとピッチングの組付目標位置に、位置・力制御させて合わせた後、既設ワークと組み付けるワークの反力により、ヨーイングを調整するアクチュエータに加わる外力をもとにして、ヨーイングの前記仮想回転軸を中心に、ヨーイング方向を補正すべく移動させて位置・力制御しながらヨーイングの組付目標位置に合わせて、ワークを最終組付目標位置に組み付けるワークの組立方法である。
【００１８】
請求項２の発明は、ワークが、シールド掘進機でトンネルを構築するセグメントが、その円周方向と軸方向の組み付け面にテーパー面を有するセグメントからなり、既設セグメントにセグメントを組み付ける際、既設セグメントに組み付ける粗位置決め目標位置までセグメントを移動したのち、既設セグメントに対する一対のガイドバーの反力により、ローリング・ピッチングを調整するアクチュエータに加わる外力をもとにして、組み付けるセグメントのロールとピッチ位置を、位置・力制御で合わせたのち、既設セグメントと組み付けるセグメントのテーパ面からの反力により、ヨーイングを調整するアクチュエータに加わる外力をもとにして、組み付けるセグメントのヨーイング位置を、位置・力制御で合わせて、セグメントを最終組付目標位置に組み付ける請求項１記載のワークの組立方法である。
【００１９】
請求項３の発明は、油圧で作動し、測長センサからなる変位センサと圧力センサが設けられた複数のアクチュエータで、多関節ロボットを構成し、そのロボットでワークを、所定の組み付け位置に移動して既設ワークに組み付けるワークの組立装置において、多関節ロボットの原点に対するワークの重心位置を、ロボット基準座標系と、組み付けるワークのワーク座標系と、組み付けの際のワークの作業に適したロール・ピッチ・ヨーの各仮想回転軸をもつ作業座標系で、それぞれ設定すると共に、これらの座標系を多関節ロボットの各アクチュエータの関節座標系で求め、組み付けるワークにローリング・ピッチングを補正する一対のガイドバーを設けると共にその組み付けるワークを、そのワーク座標の原点が粗位置決め目標位置まで、移動するよう各アクチュエータを、ロボット基準座標系を基に位置制御したのち、作業座標系に切り換え、その作業座標系をもとに、かつ上記ガイドバーの既設ワークからの反力により、ローリングとピッチングを調整するアクチュエータに加わる外力をもとにして、ワークをローリングとピッチングの前記仮想回転軸を中心にローリングとピッチング方向を補正すべく移動させて、それぞれローリングとピッチングの組付目標位置に、位置・力制御させて合わせた後、既設ワークと組み付けるワークの反力により、ヨーイングを調整するアクチュエータに加わる外力をもとにして、ヨーイングの前記仮想回転軸を中心に、ヨーイング方向を補正すべく移動させて、位置・力制御しながらヨーイングの組付目標位置に合わせて、ワークを最終組付目標位置に組み付けるための制御手段を備えたワークの組立装置である。
【００２０】
上記の構成によれば、ワークの組み付け状態に着目し、ワークを組み付ける際の作業座標系をロボットの各アクチュエータの関節座標で定義しておくことで、組み付けの際に、ワークをローリング・ピッチング・ヨーイング方向に簡単に力制御でき、組み付け位置の厳密な位置情報がなくても組み付けが可能となる。
【００２１】
【発明の実施の形態】
以下、本発明の好適実施の形態を添付図面に基づいて詳述する。
【００２２】
先ず本発明を説明する前に、先願の発明における位置・力制御を行うための装置の概略構成と制御ブロック図を、図１０，図１１で簡単に説明する。
【００２３】
図１０、図１１において、アクチュエータ１０の構成は図９で説明した通りである。
【００２４】
このアクチュエータ１０には、アブソリュート式測長センサとディジタル測長センサからなる変位センサ２９が設けられると共に圧力センサ２１が設けられる。
【００２５】
変位センサ２９は、アクチュエータ１０のロッド１８の進退位置を検出し、これを位置信号３５として速度指令生成部４０に送ると共に速度サーボ部４１にフィードバック信号として送る。また圧力センサ２１の検出圧力は速度指令生成部４０に入力される。
【００２６】
速度指令生成部４０は、変位センサ２９からの位置信号３５を基に軌道生成部４２で目標速度（ｘr ’）を生成し、同時に、圧力センサ２１の検出圧力（−ｆ）、または、外部環境２５から反力Ｆとして伝わる外力（−ｆ）を基にアドミッタンス設定部４３で、その外力ｆをアドミッタンスＢで速度信号（Ｂｆ）に変換すると共に、これらを加算器４４にて、アクチュエータ速度（ｘd ’）を、ｘr ’＝ｘ d ’−Ｂｆとなるようにして速度サーボ部４１に出力する。
【００２７】
この設定部４３でのアドミッタンスＢは、そのアクチュエータの最大押し付け力の逆数と最大速度の乗算値を基に設定され、圧力センサ２１の検出圧に応じて速度信号（Ｂｆ）を作り出すようになっている。
【００２８】
速度指令生成部４０で生成されたアクチュエータ速度（ｘd ’）は、速度サーボ部４１に入力されると、先ず、速度サーボ部４１は、アクチュエータ速度（ｘd ’）に基づいて、フィードフォワード速度信号ｖr を作り、これを加算器４５を介して速度指令値４６をサーボアンプ４７に出力し、サーボアンプ４７が制御電流４８を作ってこれをアクチュエータ１０に出力するようになっている。
【００２９】
またアクチュエータ速度（ｘd ’）は、積分器４９で、位置信号ｘr にされて比較器５０に入力され、そこで変位センサ２９からフィードバックされた位置（ｘ）信号３５と比較されて位置補償制御部５１に入力され、その位置補償制御部５１にて速度信号にされ、加算器４５にてフィードフォワード速度信号ｖr と加算されてアクチュエータ１０を作動する速度指令４６が作られる。
【００３０】
以上において、速度指令生成部４０では、アクチュエータ速度（ｘd ’）が、
ｘd ’＝ｘr ’＋Ｂｆ
となるように制御する。
【００３１】
このようにアクチュエータ速度ｘd ’を作りだしこれを速度サーボ部４１及びサーボアンプ４７を介してアクチュエータ１０を制御することで位置制御と力制御の双方を自動的に切り換えて制御することができる。
【００３２】
この図１０，図１１のアクチュエータ１０は、１軸の場合であるが、複数のアクチュエータ１０で多関節ロボットを構成する場合でも、目標方向と目標速度を、ヤコビアンを用いて各アクチュエータの運動ベクトルに分解すれば各アクチュエータごとに位置・力制御が行える。
【００３３】
さて、本発明のワーク組立方法及びその装置を説明する。
【００３４】
今、多関節ロボット６０として、シールド掘進機のセグメント組立装置のロボットを例に図４〜図７により説明する。
【００３５】
シールド掘進機は、シールドフレーム前面のカッタで地山を掘進し、その後方で、セグメント組立装置で、セグメントを円周方向に組み立ててトンネルを構築していくもので、そのセグメントとして最近、円弧状のセグメントからハニカム状のセグメントなど円周方向と軸方向の組み付け面にテーパ面を有するセグメントを組立ててトンネルを構築することがなされている。
【００３６】
図４（ａ）において、６１はエレクタリングで、ラック・ピニオン６２により旋回微調整アクチュエータ６３で、ロボット原点Ｏ0 を中心に回転され、そのエレクタリング６１に、支持部材６４が、一対の径方向アクチュエータ６５ｌ，ｒで、ロボット原点Ｏ0 に対して径方向に移動され、その支持部材６４に図４（ｂ）に示すように一対のヨーイング調整用アクチュエータ６６ｌ，ｒを介して摺動フレーム６７が、ヨーイング自在に支持され、その摺動フレーム６７に３本のローリング・ピッチング調整用アクチュエータ６８で、セグメント７０が支持される。
【００３７】
また摺動フレーム６７には坑口側に既設セグメントの内周面と接してセグメント７０のピッチング・ローリングを補正するためのガイドバー６９が設けられている。
【００３８】
このセグメント組立装置のロボットで、既設セグメントにセグメントを組み付ける手順を図６により説明する。
【００３９】
先ず、図６（ａ）に示すように既設セグメント７０ｓの組み付け位置に位置するようにセグメント７０が径方向外方に移動される。この際、セグメント７０は、ピッチング方向の姿勢が既設セグメント７０ｓの内周面と一致しない場合、ガイドバー６９の前後の接触子７１ｆ，ｂのいずれかが、図６（ｂ）、（ｃ）に示すように当たり、仮想原点Ｏβを通る軸中心にピッチし、これら接触子７１ｆ，ｂが図６（d ）に示すように均等に当たるようにピッチング方向の姿勢が調整される。
【００４０】
また、ローリング方向についても、図６（ｅ）に示すようにセグメント７０が径方向外方に移動される際に、図６（ｆ）に示すようにガイドバー６９の接触子７１ｌ，ｒのいずれかが当たり、仮想原点Ｏγを通る軸中心にロールし、これら接触子７１ｌ，ｒが図６（ｇ）に示すように均等に当たるようにローリング方向の姿勢が調整される。
【００４１】
このように、ピッチング方向とローリング方向の姿勢が調整されたセグメント７０は、図７（ａ）〜（ｃ）に示すように既設セグメント７０ｓの所定位置に組み付けるべく仮想原点Ｏαを通る軸中心にヨーし、ヨーイング方向を調整しながら所定の位置に組み付けられることとなる。
【００４２】
本発明においての組み付けの基本アルゴリズムは、ワークであるセグメント７０を、所定の組み付け位置に組み付ける際、図５（ａ），（ｂ）に示すように、先ずセグメント７０を組み付け目標位置（ｒd0，θd0，ｚd0）に位置制御で移動し、次に最終組付目標位置（ｒdc，θdc，ｚdc）まで、上述した仮想原点Ｏ（α，β，γ）を中心に、ピッチング・ローリング・ヨーイング方向を補正すべく位置・力制御しながら移動してセグメント７０を最終組付目標位置に組み付けることにある。この際、セグメント７０と共に移動する一対のガイドバー６９が、既設セグメント７０ｓに当たり、その反力をもとに、仮想原点Ｏ（α，β）を中心にピッチング・ローリング方向を補正すべく移動させて、ピッチング・ローリングの組付目標位置（ｒ dc ，θ dc ）に位置合わせを行ったのち、既設セグメント７０ｓとセグメント７０の反力をもとに、仮想原点Ｏ（γ）を中心にヨーイング方向を補正すべく位置・力制御しながら移動してヨーイングの組付目標位置（ｚ dc ）に合わせてセグメント７０を最終組付目標位置に（ｒ dc ，θ dc ，ｚ dc ）に組み付ける。
【００４３】
このセグメントを組み付けるための組み付けアルゴリズムを説明する。
【００４４】
今、上述したセグメント組立ロボットのモデル化して簡便に表すと図２に示したようになり、各アクチュエータ６３，６５，６６，６８の各関節座標系で表せる。
【００４５】
ここで、図２に示す記号と、以下で説明するアルゴリズムで使用する記号を以下に纏めて示す。計算で用いる単位系は、ＳＩ単位系を使用する。
【００４６】
(1) 座標系に関する記号
ξ ：セグメント位置座標ベクトル［ｍ］（３×１）
セグメント座標原点のロボット基準座標系で表した位置ベクトル
Ｔ ：セグメント座標姿勢行列（３×３）
セグメント座標原点のロボット基準座標系で表した回転行列
ｗ ：セグメント位置姿勢座標［ｍ／ｒａｄ］（７×１）
ｗ＝（ｘ，ｒ，θ，α，β，γ，ｈ）T （ｈ；把持部長さ）
セグメント座標系を作業座標系で表した位置／姿勢ベクトル
Ｔwi：作業座標姿勢行列（３×３）［ｉ；０〜３］
作業座標回転（０；θ回転、１；ヨー、２；ピッチ、３；ロール）
による姿勢を表す回転行列
ξ0 ：セグメント位置座標ベクトル［ｍ］（３×１）
作業座標回転０での把持位置ベクトル
ξ1 ：回転補正位置［ｍ］（３×１） ［ｉ；１〜３］
作業座標回転（１；ヨー、２；ピッチ、３；ロール）での把持位置
補正用ベクトル
ηi ：関節ｉ座標原点位置ベクトル［ｍ］（３×１） ［ｉ；１〜７］
関節ｉ座標原点を関節ｉ−ｌ座標系で表した位置ベクトル
ただし、ここで定義した座標系では以下が成立する。
【００４７】
η1 ＝η4 ＝η6 ＝Ｏ（０ベクトル）
Ａi ：関節ｉ座標姿勢行列（３×３） ［ｉ；１〜７］
関節ｉ座標姿勢を関節ｉ−ｌ座標系で表した回転行列
ただし、ここで定義した座標系では以下が成立する。
【００４８】
Ａ2＝Ａ3＝Ａ7＝Ｉ（単位行列）
η8 ：セグメント位置ベクトル［ｍ］（３×１）
セグメント座標原点を関節７座標系で表した位置ベクトル
(2)ロボット機構変数に関する記号
ｌ ：アクチュエータ変位［ｍ］（８成分）
ｌl ：旋回軸長さ、 ｌ2r：伸縮右長さ、 ｌ2l：伸縮左長さ
ｌ3r：摺動右長さ、 ｌ3l：摺動左長さ
ｌ5r：微長右長さ、 ｌ5l：微長左長さ、 ｌ5m：微長中長さ
ｑ ：関節変数ベクトル（７×１）
ｆk ：アクチュエータ変位−関節変数関係式ベクトル［ｉ；１〜４］
ｑ1 ＝ｆk1（ｌ1）
ｑ2 ＝ｆk2（ψ1）＝（ｌ2r ＋ｌ2l）／２，
ψ1＝（ｌ2r，ｌ2l）T
φ2 ＝ｆk3（ψ2）,φ2 ＝（ｑ3 ，ｑ4）T ,
ψ2＝（ｌ3r，ｌ3l）T
φ3 ＝ｆk4（ψ3）,φ3 ＝（ｑ5,ｑ6,ｑ7）T,
ψ3＝（ｌ5r,ｌ 5 ｌ,ｌ5m）T
(3)ロボット機構定数２関する記号（スカラ量）
Ｌ01 ：ヨー軸オフセット量［ｍ］
Ｌ02 ：ピッチ軸オフセット量［ｍ］
ｒ0 ：旋回微調整軸半径［ｍ］
Ｌa ：摺動左軸オフセット［ｍ］
Ｌb ：微調軸ｙオフセット［ｍ］
Ｌc ：微調軸ｚ軸オフセット［ｍ］
Ｌd ：微調軸摺動右軸オフセット［ｍ］
Ｌe ：セグメントオフセット［ｍ］
Θ2 ：摺動ステージｙ回転オフセット角［ｒａｄ］
Ω2 ：摺動ステージｘ回転オフセット角［ｒａｄ］
Θ3 ：ヨーイング軸オフセット角［ｒａｄ］
Θ4 ：摺動軸オフセット角［ｒａｄ］
さて、図１において、ロボットの原点Ｏ0 に対するロボット基準座標系を（Ｘ0 ，Ｙ0 ，Ｚ0 ）、またロボットの原点Ｏ0 に対する位置をｒ，θの円筒座標とｘ方向の座標系をとし、セグメント７０の重心位置を原点Ｏｓとし、その座標系を（Ｘｓ，Ｙｓ，Ｚｓ）とすると、仮想原点Ｏ(αβγ)は、セグメント原点Ｏｓに対してＸｓ方向でＬ01オフセットし、Ｙｓ方向にＬ02オフセットしている。
【００４９】
この仮想原点Ｏ( αβγ) におけるピッチβはＬ02オフセットしたＹβ軸廻りに、ロールγはＸγ（＝Ｘｓ）軸廻りに、ヨーαは、Ｘｓ方向にＬ01オフセットしたＺα軸廻りに回転することになる。
【００５０】
そこで、基準座標系からみたセグメント座標系の位置ξ，姿勢Ｔを、仮想原点Ｏ(αβγ)における各軸廻り、すなわちロール・ピッチ・ヨーの仮想回転軸をもつ作業座標系の位置ベクトル成分で表現する。
【００５１】
Ａ．作業座標系と基準座標系の関係式導出
(1) 各回転のみを行った場合の姿勢と位置補正量
ａ．ヨーイングＴw1は、Ｚα軸廻りの回転行列として数１で表現される。
【００５２】
【数１】
ヨーイング軸Ｚαは、セグメント原点とＬ1 だけオフセットしているので、位置ξ1 は、数２の量だけずれる。
【００５４】
【数２】
ｂ．ピッチングＴw2はＹβ軸廻りの回転行列として、数３で又、位置ξ2 は、セグメント原点とＬ2 だけオフセットしているので数４で表せる。
【００５６】
【数３】
【数４】
ｃ．ローリングＴw3は、Ｘα（＝Ｘｓ）軸廻りの回転行列として数５で表され、位置ξ3 は、オフセットが０であるため数６で表される。
【００５９】
【数５】
【数６】
(2) ｒ，θ，ｘによる位置Ｔw0と姿勢ξ0 は、数７，数８で表される。
【００６２】
【数７】
【数８】
回転順序を、ヨー，ピッチ，ロールの順で定義すると、Ｔ，ξは、下式で求められる。
【００６５】
Ｔ＝Ｔw0・Ｔw1・Ｔw2・Ｔw3 …(1)
ξ＝ξ0 ＋Ｔw0ξ1 ＋Ｔw0Ｔw1ξ2 ＋Ｔw0Ｔw1Ｔw3ξ3 …(2)
Ｂ．関節変数と基準座標系の関係式導出
次に、基準座標系からみた、セグメント座標系の位置ξ，姿勢Ｔを各アクチュエータの関節変数ｑi を用いて表現する。
【００６６】
(1) 姿勢Ｔ
各関節ごとに固定した座標系間の回転行列を定義しているので、その積で、下式のように求めることができる。
【００６７】
Ｔ＝Ａ1 ・Ａ2 ・…・Ａ7 …(3)
このＡ1 〜Ａ7 の成分は、図２で説明した関節変数ｑ1 〜ｑ7 と機構に関する定数で容易に表現できる。
【００６８】
(2) 位置ξ
各関節ごとの座標原点間のベクトルの和で下式で求めることができる。
【００６９】
ξ＝Ａ1 η1 ＋Ａ1 Ａ2 η2 ＋Ａ1 Ａ2 Ａ3 η3 ＋…
＋Ａ1 Ａ2 Ａ3 …Ａ7 （η7 ＋η8 ） …(4)
η1 〜η8 の成分は、図２で説明したように関節変数ｑと機構に関する定数で表現できる。
【００７０】
Ｃ．ヤコビ行列の導出
上記(1),(3) 式と(2),(4) 式は同じ値を示しているので、成分ごとに符号を取ると以下のようにまとめることができる。
【００７１】
ｗ＝ｆk （ｑ） … (5)
(5) 式の両辺を偏微分することによりヤコビ行列が求められる。
【００７２】
ｗ’＝Ｊｑ’ … (6)
Ｊ ＝ ｄｆk ／ｄｑ … (7)
ｑと関節駆動アクチュエータは、機構学的な関係から以下のように求められる。
【００７３】
ｑ1 ＝ ｆk1（ｌ1 ） … (8)
ｑ2 ＝ ｆk2（ψ1 ） … (9)
φ2 ＝ ｆk3（ψ2 ） …(10)
φ3 ＝ ｆk4（ψ3 ） …(11)
(8) 〜(11)式をそれぞれ偏微分すると
ｑ1 ’＝ Ｊk1・ｌ1 …(12)
ｑ2 ’＝ Ｊk2・ψ1 …(13)
φ2 ’＝ Ｊk3・ψ2 …(14)
φ3 ’＝ Ｊk4・ψ3 …(15)
また、ｑは数９なので、数９を偏微分すると数１０となる。
【００７４】
【数９】
【数１０】
次にアクチュエータ出力と作業座標の問を表すヤコビ行列は、下式で求められる。
【００７７】
ｗ’＝Ｊ・Ｊｑ・ｌ’ …(16)
よってヤコビ行列を用いることで、作業座標系での力制御を実現できるようになる。
【００７８】
図３は、本発明のワーク（セグメント）と組み立てるための位置／力のハイブリッド制御系のブロック図を示す。
【００７９】
この図３の制御系を説明する前に力制御アルゴリズムに関する記号を説明する。
【００８０】
ｐ ：アクチュエータ発生圧力［Ｐａ］（８×２成分）
ｆl ：アクチュエータ発生力ベクトル［Ｎ］（７×１）
ｆ ：セグメント発生力［Ｎ／Ｎｍ］（７×１）
ｆ＝（ｆx ，ｆr ，ｍθ，ｍα，ｍβ，ｍγ，ｆg ）T
（ロボット作業座標系ベクトル／ｆg ；把持力）
ｖr ：アクチュエータ指令速度［ｍ／ｓ］（８×１）
ｗd ：力フィードバック速度［（ｍ／ｓ）／（ｒａｄ／ｓ）］（７×１）
ｗr ：力制御目標速度［（ｍ／ｓ）／（ｒａｄ／ｓ）］（７×１）
ｌ’r ：アクチュエータ目標速度［ｍ／ｓ］（８×１）
ｖd ：アクチュエータ指令速度［ｍ／ｓ］（８×１）
Ｊ ：ヤコビ行列（８×１）
Ｊ＝ｄｆk ／ｄｑ
ｗ＝ｆk （ｑ）；順運動学関係式ベクトル（７×１）
（ｗ’＝Ｊｑ’）
Ｊ-1 ：ヤコビ行列逆行列（７×７）
ＪT ：ヤコビ行列転置逆行列（７×７）
Ｂ ：アドミッタンス行列［(m/s)/Ｎ／(rad/s)/Ｎｍ］（７×７; 対角行列）
Ｓ ：力制御選択行列（７×７; 対角行列）
Ｉ ：単位行列（７×７）
Ｊi ：関節変数行列
Ｊ1 ＝ｄｆk1／ｄｌ1 （スカラ関数）
Ｊ2 ＝ｄｆk2／ｄψ2 （２×１行列）
Ｊ3 ＝ｄｆk3／ｄψ2 （２×２行列）
Ｊ4 ＝ｄｆk1／ｄψ3 （３×３行列）
さて今、理想的な力のセンシングができればダンピング制御を用いた力制御で位置制御も可能であるが、現実にはセンサのノイズの影響があり、厳密な位置決め制御は実現できない。
【００８１】
組立作業において、接触のない方向に対しては力制御の必要がないことから、位置制御／力制御を選択できる制御系を構成する。
【００８２】
そこで、力制御選択行列Ｓを以下のように定義し、下式に示す制御アルゴリズムで実現できる。
【００８３】
ｗr ’＝Ｓ（ｗd ’＋Ｂｆ）＋（Ｉ−Ｓ）ｗd ’ …(17)
Ｓ＝diag［s1，s2，・・，s7］
si＝１：ｉ軸力制御選択
si＝０：ｉ軸位置制御選択
Ｉ：単位行列
上記(17)式によって得られた力制御目標速度を(16)式で各アクチュエータの指令速度に変換し、サーボ系を構成することによって力制御が実現できる。
【００８４】
このようにして構成された制御ブロック図を図３で説明する。
【００８５】
図３において、速度指令生成部で生成される目標速度ｗd'が軌道計画生成部７５で生成し、これを位置制御選択行列（Ｉ−Ｓ）７６と加算器７７を介して力制御行列（Ｓ）７８に出力する。ここでＩは単位行列、Ｓは選択行列で、それぞれその要素が１または０の対角行列であり、１は、それに対応する座標でアドミッタンス行列（Ｂ）７９を基に力制御をすることを意味し、０は、位置制御を行うことを意味している。
【００８６】
位置制御選択行列（Ｉ−Ｓ）７６と力制御行列（Ｓ）７８の出力は、加算器８０にて加算されて力制御目標速度ｗｒとされ、ヤコビ行列逆行列８１にてそれぞれアクチュエータごとの運動ベクトルに分解される。
【００８７】
制御の開始時には、反力Ｆ＝０であり、位置行列選択７６で生成される座標がヤコビ行列逆行列８１より、速度制御補償部８２にて各アクチュエータ１０の速度信号ｖr が生成され、加算器８３を介してフィードフォワード速度信号としてアクチュエータ１０に出力される。一方ヤコビ行列逆行列８１にて分解された運動ベクトルは積分器８４で、各アクチュエータのシリンダ位置信号８５にされて比較器８６に入力され、そこで各アクチュエータ１０からフィードバックされたシリンダ変位信号（Ｉ）８７と比較されて位置補償制御部８８に入力され、その位置補償制御部８８にて速度信号にされ、加算器８３にてフィードフォワード速度信号ｖr と加算されて各アクチュエータ１０を作動する速度指令Ｖｄが作られ、その速度指令Ｖｄに基づいてアクチュエータ１０からなるセグメントロボットが、目標軌道に沿った速度ｗr で移動して外部環境２５に接触するまで位置制御する。
【００８８】
次に外部環境２５にワーク２４が当たり移動が停止して、速度ｘr ’が０となると、各アクチュエータ１０の圧力センサからシリンダ発生力（Ｆｌ）９０が、ヤコビ行列転置逆行列９１に送られて外力Ｆに変換され、アドミッタンス行列７９により外力Ｆが速度ベクトル（ＢＦ）にされ、力制御選択行列７８により力制御に切り換えられ、セグメントに設定の力が加えられることなる。
【００８９】
この制御において、上述したようにセグメントを粗位置決め目標位置に移動すると、既設セグメントなどの外部環境２５より、位置制御から力制御に切り換える際のピッチング・ローリング・ヨーイング方向の調整は、図６に示すように、位置制御のときのアクチュエータ速度ｖ1rが、力制御のときのセグメント発生力ｆr でピッチング方向の反力ｆ1rf ，ｆ1rb とローリング方向の反力ｆ1rr ，ｆ1rl が均等になるためのセグメント発生力ｆ1rとなるように力制御すれば良く、また同様にヨーイングに関しても、図７に示したように、速度ｖ2zで位置制御し、セグメント発生力ｆz で力制御したとき、ヨーイング方向の反力ｆ2z1 ，ｆ2z2 が釣合い、反力ｆ2zo でｆ2zの発生力となったときに、正しくセグメント７０が組み付けられたとして制御を完了する。
【００９０】
このように本発明においては、粗位置決め目標位置から、位置・力制御により最終組付目標位置までピッチ・ロール・ヨーを調整しながらセグメントを正確に組み付けることが可能となる。
【００９１】
上述の実施の形態においては、組立てるワークとしてシールド掘進機のトンネル構築用のセグメントの例で説明したが、セグメントに限らず種々のワークの組立に適用できる。また、組み付けの際にロール・ピッチ・ヨーを行い、ヨーイングを最後に行う例で説明したが、これらはワークの組み付け方や形状によっていずれの順序で行っても良いし、いずれか１つか２つでも良い。
【００９２】
【発明の効果】
以上要するに本発明によれば、変位センサと圧力センサが設けられた複数のアクチュエータで、多関節ロボットを構成し、多関節ロボットの原点に対するワークの重心位置を、ロボット基準座標系と、組み付けるワークのワーク座標系と、組み付けの際のワークの作業に適したロール・ピッチ・ヨーの各仮想回転軸をもつ作業座標系で、それぞれ設定すると共に、これら座標系を多関節ロボットの各アクチュエータの関節座標で求め、組み付けるワークに一対のガイドバーを設け、ワークを組み付ける際に、ロボット基準座標系とワーク座標系で、粗置決め目標位置まで移動したときに、作業座標系に切り換え、ガイドバーから各アクチュエータに加わる外力をもとに、位置・力制御でワークのロール・ピッチの組付目標位置に調整した後、既設ワークからの反力をもとに、位置・力制御でワークのヨーの組付目標位置を調整することで、組み付け位置の厳密な位置情報がなくても最終組付目標にワークを組み付けることが可能となる。
【図面の簡単な説明】
【図１】本発明において、作業座標系の設定を説明する概略図である。
【図２】本発明におけるロボットの関節座標系を説明する概略図である。
【図３】本発明における位置・力制御の制御ブロック図を示す図である。
【図４】本発明において、ロボットとしてのセグメント組立装置を示す概略図である。
【図５】本発明において、粗位置決めと組み付け目標位置の概念図を示す図である。
【図６】本発明において、ピッチングとローリング補正を説明する概略図である。
【図７】本発明において、ヨーイング・旋回位置補正を説明する概略図である。
【図８】先願の発明におけるアクチュエータ位置・力制御を説明する概念図である。
【図９】先願及び本発明に用いるアクチュエータの詳細を示す図である。
【図１０】先願の発明におけるシリアルリンクの制御装置を説明する概略図である。
【図１１】先願の発明におけるアクチュエータの制御ブロック図を示す図である。
【符号の説明】
１０ アクチュエータ
７０ セグメント（ワーク）
Claims (3)
- 油圧で作動し、測長センサからなる変位センサと圧力センサが設けられた複数のアクチュエータで、多関節ロボットを構成し、そのロボットでワークを、所定の組み付け位置に移動して既設ワークに組み付けるワークの組立方法において、多関節ロボットの原点に対するワークの重心位置を、ロボット基準座標系と、組み付けるワークのワーク座標系と、組み付けの際のワークの作業に適したロール・ピッチ・ヨーの各仮想回転軸をもつ作業座標系で、それぞれ設定すると共に、これらの座標系を多関節ロボットの各アクチュエータの関節座標系で求め、組み付けるワークにローリング・ピッチングを補正する一対のガイドバーを設けると共にその組み付けるワークを、そのワーク座標の原点が粗位置決め目標位置まで、移動するよう各アクチュエータを、ロボット基準座標系を基に位置制御したのち、作業座標系に切り換え、その作業座標系をもとに、かつ上記ガイドバーの既設ワークからの反力により、ローリングとピッチングを調整するアクチュエータに加わる外力をもとにして、ワークをローリングとピッチングの前記仮想回転軸を中心にローリングとピッチング方向を補正すべく移動させて、それぞれローリングとピッチングの組付目標位置に、位置・力制御させて合わせた後、既設ワークと組み付けるワークの反力により、ヨーイングを調整するアクチュエータに加わる外力をもとにして、ヨーイングの前記仮想回転軸を中心に、ヨーイング方向を補正すべく移動させて位置・力制御しながらヨーイングの組付目標位置に合わせて、ワークを最終組付目標位置に組み付けることを特徴とするワークの組立方法。
- ワークが、シールド掘進機でトンネルを構築するセグメントが、その円周方向と軸方向の組み付け面にテーパー面を有するセグメントからなり、既設セグメントにセグメントを組み付ける際、既設セグメントに組み付ける粗位置決め目標位置までセグメントを移動したのち、既設セグメントに対する一対のガイドバーの反力により、ローリング・ピッチングを調整するアクチュエータに加わる外力をもとにして、組み付けるセグメントのロールとピッチ位置を、位置・力制御で合わせたのち、既設セグメントと組み付けるセグメントのテーパ面からの反力により、ヨーイングを調整するアクチュエータに加わる外力をもとにして、組み付けるセグメントのヨーイング位置を、位置・力制御で合わせて、セグメントを最終組付目標位置に組み付ける請求項１記載のワークの組立方法。
- 油圧で作動し、測長センサからなる変位センサと圧力センサが設けられた複数のアクチュエータで、多関節ロボットを構成し、そのロボットでワークを、所定の組み付け位置に移動して既設ワークに組み付けるワークの組立装置において、多関節ロボットの原点に対するワークの重心位置を、ロボット基準座標系と、組み付けるワークのワーク座標系と、組み付けの際のワークの作業に適したロール・ピッチ・ヨーの各仮想回転軸をもつ作業座標系で、それぞれ設定すると共に、これらの座標系を多関節ロボットの各アクチュエータの関節座標系で求め、組み付けるワークにローリング・ピッチングを補正する一対のガイドバーを設けると共にその組み付けるワークを、そのワーク座標の原点が粗位置決め目標位置まで、移動するよう各アクチュエータを、ロボット基準座標系を基に位置制御したのち、作業座標系に切り換え、その作業座標系をもとに、かつ上記ガイドバーの既設ワークからの反力により、ローリングとピッチングを調整するアクチュエータに加わる外力をもとにして、ワークをローリングとピッチングの前記仮想回転軸を中心にローリングとピッチング方向を補正すべく移動させて、それぞれローリングとピッチングの組付目標位置に、位置・力制御させて合わせた後、既設ワークと組み付けるワークの反力により、ヨーイングを調整するアクチュエータに加わる外力をもとにして、ヨーイングの前記仮想回転軸を中心に、ヨーイング方向を補正すべく移動させて、位置・力制御しながらヨーイングの組付目標位置に合わせて、ワークを最終組付目標位置に組み付けるための制御手段を備えたことを特徴とするワークの組立装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP28358695A JP3967394B2 (ja) | 1995-10-31 | 1995-10-31 | ワークの組立方法及びその装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP28358695A JP3967394B2 (ja) | 1995-10-31 | 1995-10-31 | ワークの組立方法及びその装置 |
Related Child Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2002294677A Division JP3780245B2 (ja) | 2002-10-08 | 2002-10-08 | シールド掘進機のセグメント組立方法及びその装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH09128023A JPH09128023A (ja) | 1997-05-16 |
| JP3967394B2 true JP3967394B2 (ja) | 2007-08-29 |
Family
ID=17667435
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP28358695A Expired - Fee Related JP3967394B2 (ja) | 1995-10-31 | 1995-10-31 | ワークの組立方法及びその装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3967394B2 (ja) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110700865A (zh) * | 2019-11-25 | 2020-01-17 | 盾构及掘进技术国家重点实验室 | 一种基于tbm智能机器人的钢拱架支护系统及控制方法 |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2004263645A (ja) * | 2003-03-03 | 2004-09-24 | Opton Co Ltd | 液圧装置 |
| JP7082477B2 (ja) * | 2017-11-22 | 2022-06-08 | 古河機械金属株式会社 | 制御装置、制御方法及びプログラム |
| JP7082476B2 (ja) * | 2017-11-22 | 2022-06-08 | 古河機械金属株式会社 | 制御装置、制御方法及びプログラム |
-
1995
- 1995-10-31 JP JP28358695A patent/JP3967394B2/ja not_active Expired - Fee Related
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110700865A (zh) * | 2019-11-25 | 2020-01-17 | 盾构及掘进技术国家重点实验室 | 一种基于tbm智能机器人的钢拱架支护系统及控制方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| JPH09128023A (ja) | 1997-05-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US4300198A (en) | Robot with light-weight, inertia-free programming device | |
| Bayani et al. | An experimental study on the vision-based control and identification of planar cable-driven parallel robots | |
| Dunnigan et al. | Hybrid position/force control of a hydraulic underwater manipulator | |
| CN112894821A (zh) | 基于电流法的协作机器人拖动示教控制方法、装置及设备 | |
| JP3967394B2 (ja) | ワークの組立方法及びその装置 | |
| Sangveraphunsiri et al. | Dynamics and control of a 5-DOF manipulator based on an H-4 parallel mechanism | |
| Williams et al. | Planar parallel 3-RPR manipulator | |
| Chin et al. | Force computation and continuous path tracking for hydraulic parallel manipulators | |
| JP3780245B2 (ja) | シールド掘進機のセグメント組立方法及びその装置 | |
| Zarkandi et al. | PRRRRRP redundant planar parallel manipulator: Kinematics, workspace and singularity analysis | |
| Lee et al. | A novel 4-DoF robotic link mechanism with e-CoSMo: Kinematics based torque analysis | |
| Guihard et al. | SAPPHYR: Legs to pull a wheel structure | |
| Pessi et al. | Accuracy analysis of hybrid parallel robot for the assembling of ITER | |
| JP3331674B2 (ja) | ７軸多関節ロボットマニピュレータの肘回転方法 | |
| CN116533227A (zh) | 一种基于示教轨迹优化与修正的机器人打磨系统的打磨方法 | |
| JP3248821B2 (ja) | アクチュエータの作動方法及び装置 | |
| JPH11191005A (ja) | ロボット制御装置 | |
| Li et al. | Robust control of a two-link flexible manipulator with neural networks based quasi-static deflection compensation | |
| Tsai et al. | Fuzzy logic control of a planetary gear type walking machine leg | |
| JPH0760667A (ja) | ロボットのウィービング制御装置 | |
| Mills | Hybrid actuation of robotic manipulators: an integral mainfold control approach | |
| Su et al. | Force control of high-speed, lightweight robotic manipulators | |
| JP2583272B2 (ja) | ロボットの制御装置 | |
| JPS6365507A (ja) | 多関節作業機の軌跡制御装置 | |
| JP7182952B2 (ja) | 制御方法、制御プログラム、記録媒体、制御装置、ロボットシステム、および物品の製造方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20040629 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20040830 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20050315 |
|
| A521 | Written amendment |
Effective date: 20050516 Free format text: JAPANESE INTERMEDIATE CODE: A523 |
|
| A911 | Transfer of reconsideration by examiner before appeal (zenchi) |
Effective date: 20050519 Free format text: JAPANESE INTERMEDIATE CODE: A911 |
|
| A912 | Removal of reconsideration by examiner before appeal (zenchi) |
Free format text: JAPANESE INTERMEDIATE CODE: A912 Effective date: 20050902 |
|
| A521 | Written amendment |
Effective date: 20070403 Free format text: JAPANESE INTERMEDIATE CODE: A523 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20070531 |
|
| R150 | Certificate of patent (=grant) or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Year of fee payment: 3 Free format text: PAYMENT UNTIL: 20100608 |
|
| S533 | Written request for registration of change of name |
Free format text: JAPANESE INTERMEDIATE CODE: R313533 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100608 Year of fee payment: 3 |
|
| R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100608 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Year of fee payment: 4 Free format text: PAYMENT UNTIL: 20110608 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Year of fee payment: 4 Free format text: PAYMENT UNTIL: 20110608 |
|
| S111 | Request for change of ownership or part of ownership |
Free format text: JAPANESE INTERMEDIATE CODE: R313115 |
|
| R371 | Transfer withdrawn |
Free format text: JAPANESE INTERMEDIATE CODE: R371 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Year of fee payment: 4 Free format text: PAYMENT UNTIL: 20110608 |
|
| S111 | Request for change of ownership or part of ownership |
Free format text: JAPANESE INTERMEDIATE CODE: R313115 |
|
| S111 | Request for change of ownership or part of ownership |
Free format text: JAPANESE INTERMEDIATE CODE: R313115 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Year of fee payment: 4 Free format text: PAYMENT UNTIL: 20110608 |
|
| R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120608 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Year of fee payment: 6 Free format text: PAYMENT UNTIL: 20130608 |
|
| LAPS | Cancellation because of no payment of annual fees |