JPH09128023A - ワークの組立方法及びその装置 - Google Patents

Abstract

(57)【要約】 【目的】 位置・力制御を行うにおいてワークを簡単に
組み立てるワークの組立方法及びその装置を提供する。 【構成】 油圧で作動する複数のアクチュエータで、多
関節ロボットを構成し、そのロボットでワーク７０を、
所定の組み付け位置に移動して組み立てるワーク７０の
組立方法において、ロボットの原点に対するロボット基
準座標系（Ｘ0 ，Ｙ0 ，Ｚ0 ）と、組み付けるワークの
ワーク座標系（Ｘｓ，Ｙｓ，Ｚｓ）と、組み付けの際の
ワークの作業に適したロール・ピッチ・ヨーの仮想回転
軸（Ｚγ，Ｙβ，Ｘγ）作業座標系とをそれぞれ設定す
ると共に、これらをロボットの各アクチュエータ１０の
関節座標系で求め、ワーク７０を、そのワーク座標の原
点が目標粗位置まで、移動するように各アクチュエータ
を位置制御したのち、作業座標系をもとに組み付け位置
にワークをローリング・ピッチング・ヨーイング方向に
位置・力制御しながらワークを組み立てることを特徴と
している。

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 ）に示すように均等
に当たるようにピッチング方向の姿勢が調整される。

【００４０】また、ローリング方向に付いても、図６
（ｅ）に示すようにセグメント７０が径方向外方に移動
される際に、図７（ｆ）に示すようにガイドバー６９の
接触子７１ｌ，ｒのいずれかが当たり、仮想原点Ｏγを
通る軸中心にロールし、これら接触子７１ｌ，ｒが図６
（ｇ）に示すように均等に当たるようにローリング方向
の姿勢が調整される。

【００４１】このように、ピッチング方向とローリング
方向の姿勢が調整されたセグメント７０は、図７（ａ）
〜（ｃ）に示すように既設セグメント７０ｓの所定位置
に組み付けるべく仮想原点Ｏαを通る軸中心にヨーし、
ヨーイング方向を調整しながら所定の位置に組み付けら
れることとなる。

【００４２】本発明においての組み付けの基本アルゴリ
ズムは、ワークであるセグメン７０を、所定の組み付け
位置に組み付ける際、図５（ａ），（ｂ）に示すよう
に、先ずセグメント７０を組み付け目標位置（ｒdc，θ
dc，ｚdc）に位置制御で移動し、次に組付目標位置（ｒ
d0，θd0，ｚd0）まで、上述した仮想原点Ｏ(α,β,γ)
を中心に、ピッチング・ローリング・ヨーイング方向に
位置・力制御しながらセグメント７０を組み付けること
にある。

【００４３】このセグメントを組み付けるための組み付
けアルゴリズムを説明する。

【００４４】今、上述したセグメント組立ロボットをモ
デル化しての簡便に表すと図２に示したようになり、各
アクチュエータ６３，６５，６６，６８の各関節座標系
で表せる。

【００４５】ここで、図２に示す記号と、以下で説明す
るアルゴリズムで使用する記号を以下に纏めて示す。計
算で用いる単位系は、ＳＩ単位系を使用する。

【００４６】(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, ｌ5r, ｌ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の発生力となったときに、正しくセグメ
ント７０が組み付けられたとして制御を完了する。

【００９０】このように本発明においては、粗位置決め
位置から、位置・力制御により組み付け目標位置までピ
ッチ・ロール・ヨーを調整しながらセグメントを正確に
組み付けることが可能となる。

【００９１】上述の実施の形態においては、組立てるワ
ークとしてシールド掘進機のトンネル構築用のセグメン
トの例で説明したが、セグメントに限らず種々のワーク
の組立に適用できる。また、組み付けの際にロール・ピ
ッチ・ヨーを行い、ヨーイングを最後に行う例で説明し
たが、これらはワークの組み付け方や形状によっていず
れの順序で行っても良いし、いずれか１つか２つでも良
い。

【００９２】

【発明の効果】以上要するに本発明によれば、ワークの
組み付け状態に着目し、ワークを組み付ける際の作業座
標系をロボットの各アクチュエータの関節座標で定義し
ておくことで、組み付けの際に、ワークをロール・ピッ
チ・ヨー方向に簡単に力制御でき、組み付け位置の厳密
な位置情報がなくても組み付けが可能となる。

【図面の簡単な説明】

【図１】本発明において、作業座標系の設定を説明する
概略図である。

【図２】本発明におけるロボットの関節座標系を説明す
る概略図である。

【図３】本発明における位置・力制御の制御ブロック図
を示す図である。

【図４】本発明において、ロボットとしてのセグメント
組立装置を示す概略図である。

【図５】本発明において、粗位置決めと組み付け目標位
置の概念図を示す図である。

【図６】本発明において、ピッチングとローリング補正
を説明する概略図である。

【図７】本発明において、ヨーイング・旋回位置補正を
説明する概略図である。

【図８】先願の発明におけるアクチュエータ位置・力制
御を説明する概念図である。

【図９】先願及び本発明に用いるアクチュエータの詳細
を示す図である。

【図１０】先願の発明におけるシリアルリンクの制御装
置を説明する概略図である。

【図１１】先願の発明におけるアクチュエータの制御ブ
ロック図を示す図である。

【符号の説明】

１０ アクチュエータ ７０ セグメント（ワーク）

フロントページの続き (72)発明者 村上 弘記 東京都江東区豊洲三丁目１番15号 石川島 播磨重工業株式会社東二テクニカルセンタ ー内 (72)発明者 北山 仁志 愛知県知多市北浜町11番１号 石川島播磨 重工業株式会社愛知工場内 (72)発明者 富松 宏明 愛知県知多市北浜町11番１号 石川島播磨 重工業株式会社愛知工場内 (72)発明者 福田 敏男 愛知県名古屋市東区矢田町２−66 名大矢 田宿舎122 (72)発明者 小菅 一弘 宮城県仙台市泉区館３丁目21番10号

Claims (3)

【特許請求の範囲】
1. 【請求項１】 油圧で作動する複数のアクチュエータ
   で、多関節ロボットを構成し、そのロボットでワーク
   を、所定の組み付け位置に移動して組み立てるワークの
   組立方法において、ロボットの原点に対するロボット基
   準座標系と、組み付けるワークのワーク座標系と、組み
   付けの際のワークの作業に適したロール・ピッチ・ヨー
   の仮想回転軸をもつ作業座標系とをそれぞれ設定すると
   共に、これらをロボットの各アクチュエータの関節座標
   系で求め、ワークを、そのワーク座標の原点が目標粗位
   置まで、移動するよう各アクチュエータを位置制御した
   のち、作業座標系をもとに組み付け位置にワークをロー
   リング・ピッチング・ヨーイング方向に位置・力制御し
   ながらワークを組み立てることを特徴とするワークの組
   立方法。
2. 【請求項２】 ワークが、シールド掘進機でトンネルを
   構築する組み付け面にテーパ面を有するセグメントから
   なり、既設セグメントにセグメントを組み付ける際、既
   設セグメントに組み付ける目標粗位置までセグメントを
   移動したのち、そのセグメントのワークのロールとピッ
   チを合わせたのち、ヨーしながらセグメントを組み付け
   る請求項１記載のワークの組立方法。
3. 【請求項３】 油圧で作動する複数本のアクチュエータ
   で多関節ロボットを構成し、そのロボットで、ワークを
   所定の位置に組み付ける組立装置において、ロボットの
   原点に対するロボット基準座標系と、組み付けるワーク
   のワーク座標系と、組み付けの際のワークのロール・ピ
   ッチ・ヨーの仮想回転軸をもつ作業座標系とをそれぞれ
   設定すると共に、これらをロボットの各アクチュエータ
   の関節座標系で求め、ワークを、そのワーク座標の原点
   が目標粗位置まで、移動するよう各アクチュエータを位
   置制御したのち、作業座標系をもとに組み付け位置にワ
   ークをローリング・ピッチング・ヨーイング方向に位置
   ・力制御しながらワークを組み立てることを特徴とする
   ワークの組立装置。