JPH079608B2 - ロボツトの制御方法及びその装置 - Google Patents

Description

【発明の詳細な説明】 〔発明の利用分野〕 本発明は、ロボットの制御方法及びその装置に係り、特
に多自由度に動作するロボットに種々の作業をさせる
際、好適な制御方法及びその装置に関するものである。

〔発明の背景〕

近年、ロボットは様々な分野に広く導入されているが、
これはロボットが一般の専用機械と異なり汎用性に富
み、多種多様な作業に適用し得ることが大きな特徴の１
つであるからである。

ロボットをある作業に適用する場合、そのままの形で使
用される例は少なく、取り扱う対象物に応じたツールを
ロボットに備え付けて使用するのが一般的である。

例えば、ロボットを現場の加工又は組立ラインに導入し
作業させる場合、対象物の形や大きさ、あるいは作業の
内容に応じたツールをロボットの先端にあるハンド部や
アーム部とハンド部の間に設け、組立作業や塗装作業等
を行なわせている。この場合、ツール自体にある程度の
柔軟性を持たせて、対象物の精度上のバラツキや位置決
め誤差等作業不良の要因を吸収・排除することが行なわ
れている。

しかしながら、作業内容が複雑かつ高度なものになるに
伴い、ツール自体も自由度が高く高機能を要求され、従
ってその構造も複雑で高価なものになる傾向にある。さ
らに１つのツール自体では対処し切れず、同じ作業でも
対象物の形状や作業条件により、ツール交換も必要とな
ることも少なくない。

この結果、ツール交換及びその調整に時間がかかりロボ
ットの稼動率を低下させるだけでなく、作業の信頼性を
保持するためにツールの保守・管理が生ずる等幾多の問
題点を派生する。

柔軟性を有するツールの例として、USP 4,098,001に開
示されているRemote Center Compliance（以下RCCとい
う）機構がある。このRCC機構は、把持された対象物の
先端に力やモーメントが加わると、平行移動や回転して
対処し、ロボットハンドを単に直線的に移行させるだけ
で、例えば軸状部品を挿入穴にはめ合わせることができ
るものである。しかし、このRCC機構も対象物である軸
状部品の長さやはめ合い条件の変化に対する柔軟性は充
分ではない。

以上述べた様に、ロボットとは、従来の専用機械が単一
の部品、作業しか対象とできなかった点と異なり、多品
種の部品を扱い、異なる作業を同一の機械で可能である
点に、その大きな意義がある。このため、ロボットが品
種、作業が変わる度にツールの交換が必要であるとする
と、汎用性あるいはフレキシビリティの面からロボット
としての特徴を誇示しにくい。

しかし、このようなツール交換或いはこれと同等の事を
現在のロボット技術では行なわざるを得ないのが実情で
あり、これに対処するために、ツールに汎用性を持たせ
ることが考えられるが、汎用性を持つツールとは、往々
にして機構が複雑であり、不必要な部分を含む場合が多
く、ツール交換を行なう場合より、むしろ無駄が多くな
ることがある。

このように、機構だけで解決しようとすると逆に、この
フレキシビリティという問題は、より難しくなるという
性質を持つ。

〔発明の目的〕

本発明の目的は、種々の作業条件に対してツール交換等
の機構的によらず、制御上の特性パラメータを変更する
だけで対処し得るロボットの制御方法及びその装置を提
供するにある。

〔発明の概要〕

本発明は、サンプリングタイム毎にロボットを制御する
ロボットの制御方法であって、前サンプリングタイム
（ｎ−１）におけるロボットのハンドに加えられた実際
の力を検知して力信号を算出し、この信号と前サンプリ
ングタイム（ｎ−１）における目標値であった力基準値
との差である第１の値を算出し、前記ハンドの前サンプ
リングタイム（ｎ−１）の実際の位置及び姿勢を表わす
位置信号と前サンプリングタイム（ｎ−１）における位
置及び姿勢の目標値であった位置基準値との差にバネ定
数を掛けた第２の値を算出し、前記ハンドの前サンプリ
ングタイム（ｎ−１）における速度信号としてロボット
に設けられた位置検出器からの位置検出値を微分した値
を用い、これに粘性定数を掛けた第３の値を算出し、前
記第1,第2,第３の値と前記ハンドの質量係数から前記ハ
ンドの次サンプリングタイム（ｎ）の指令速度を算出す
るようにしたものである。

また、位置信号として、前記ロボットに設けられた位置
検出器からの位置検出値を使用するようにしたものであ
る。

また、位置検出信号として、前記ロボットに設けられた
非接触センサからの位置情報を使用するようにしたもの
である。

さらに、ロボットの制御装置をロボットのハンドに加え
られえた力に基づいて力信号を算出する第１の算出手段
と、前記ロボットに設けられた位置検出器からの情報に
基づいて前記ハンドの位置及び姿勢を表わす位置信号を
算出する第２の算出手段と、前記力信号及び前記位置信
号に基づいて、前記ハンドの指令速度から前記ロボット
の関節角速度を算出する第４の算出手段から構成したも
のである。

〔発明の実施例〕

以下、本発明を図に基づいて具体的に説明する。

第１図は、関節形ロボットと制御系の全体構成の概略を
示す構成図である。ここで、制御対象のロボットの一例
として示された関節形ロボット10は、ベース上を垂直軸
まわりに旋回可能な旋回台11と、この旋回台11上に水平
軸を中心に回動可能に設けられた上腕12と、この上腕12
の先端部に位置する他の水平軸を中心に回動可能に設け
られた前腕13と、この前腕13の先端部に図示の如く３自
由度に動作する手首部14を有する計６自由度の関節形ロ
ボットである。この手首部14には取り扱う対象物を把持
するハンド15及びロボットに加えられる力を検出する力
センサ16を有する。

なお、前記旋回台11,上腕12,前腕13及び手首部14を各々
駆動するモータＭには、各々の回転角度を検出するため
のエンコーダPE及び回転速度を検出するためのタコジェ
ネレータTGが連結されている。

制御装置17はエンコーダPE及び力センサ16からそれぞれ
得られるロボットの位置信号及びロボットに加えられ
た力信号ｖを取り込み、予め入力された信号基準値
ｒ及び力基準値ｃとからロボットの を算出してサーボアンプ18に対して出力し、関節形ロボ
ット10を作動させる。

第２図は、この制御装置とロボットの駆動部の各構成要
素及びその関連を示する説明図である。制御装置17は各
種の演算及び制御を行なうマイクロプロセッサ19と、RA
M20と、ROM21と、定型の関数演算を行なう演算部22と、
操作卓23に接続するインターフェース回路（I/F）24
と、これらの各構成要素を接続するバスライン25を有
し、さらにバスライン25にはサーボアンプ18を介してロ
ボット駆動部26にあるモータＭに出力するため、角速度
のディジタル信号をアナログ信号に交換するD/A変換回
路27と、モータＭに連結されたエンコーダPEからの出力
を計数するカウンタ28,力センサ16からアンプ29を介し
て出力された力に関するアナログ信号をディジタル信号
に変換するためのA/D変換回路30とが接続されている。

ここで、位置基準値ｒ及び力基準値ｃは０でもよい
が、この場合関節形ロボット10はハンド15に与えられる
力ｖによってのみ動作することになる。即ち、受動的
な機構となる。

一方、位置基準値ｒ及び力基準値ｃのみが与えられ
たときは、これらの基準値によって関節形ロボット10の
動きが定められる。

以上の構成により制御装置17は、次の処理動作を行な
う。

第３図は制御装置17の処理動作を示す概略ブロック線図
である。

制御装置17は、位置・姿勢算出部40においてロボットの
各関節のエンコーダPEからの信号をカウンタ28で計数し
てロボットの位置信号を受け、この各関節の角度から
ハンド系の直交座標に座標変換して、ハンドの位置・
姿勢を算出する。実際には第２図におけるマイクロプロ
セッサ19からの指令で演算部22等が行なう。また、ロボ
ットに加わる外力ａを力センサ16が検出し、力センサ
16を構成している歪ゲージの電圧ｖとして、制御装置
17は、アンプ29及びA/D変換回路30を介して取り込む。
力算出部50は、v,を取り込み、まず歪ゲージの電圧
ｖを歪／力の変換マトリックス等により力に変換す
る。実際には、上記と同様第２図におけるマイクロプロ
セッサ19からの指令で演算部22等が行なう。

次に、ハンド15及びハンド15が把持する物の重量が大き
い場合には、ハンドの姿勢によって力センサ16に与える
値が異なる。このため、ハンド15の姿勢をより算出
し、これによる影響を取り除き、実際にロボットに加っ
た力ａを推定する。こうして推定された値がであ
り、制御主要部60に与えられる。制御主要部60は、この
算出された力及びロボットの位置と予め入力される
力及び位置の基準値c,ｒからハンド15が動作すべき
速度を算出する。この速度は、ハンドに固定された座標
系での値であるので、これを座標変換によってベースの
座標系での速度に変換されて出力される。

この後、直交座標系での速度から関節角速度算出部70
によってロボット各軸が取るべき が出力される。ただし、 （ここにJ^-1は逆ヤコビアン行列と呼ばれ、その要素は
ロボットの位置信号の関数である。）この はD/A変換回路27でアナログ信号に変換され、サーボア
ンプ18を介してロボットの各関節用のモータＭに出力さ
れ、ロボットを動作させる。

以上、制御装置17の動作を述べたが、このうち制御主要
部60の処理方法が主に本発明の特徴を成すもので、以下
にその原理的な説明を行なう。

一般に、柔軟性を有するツールの動きは慣性力、バネに
よる復元力ダンパによる粘性力と加えられる力によって
関係づけられる。ここでは説明を簡単にするため１自由
度について述べる。

１自由度のツールのモデルを数式で示すと次の（１）式
の如くとなる。

ここで、ｍはハンドの質量,Kはバネのバネ係数,Cはダン
パの粘性係数であり、xr,Fcはそれぞれ位置と力の基準
値である。

（１）式は（Ｆ−Fc）なる力がツールに加えられたとき
のツールの位置及び速度がそれぞれｘ及びｖとなること
を示す。言い換えると、もしロボットのハンドに（Ｆ−
Fc）なる力を加えたとき、ロボットのハンドがｘ及びｖ
なる位置、及び速度を持ち、ロボットのハンドは実際に
はツールを装着していないにも拘らず、ツールを装着し
たときと同様の運動をさせることができることを示すも
のである。

即ち、ロボットに装着させたいツールを想定してロボッ
トを制御させることによって、そのツールをロボットに
組込んだときにするであろうハンドの動きをロボットに
させることができるもので、このことが本発明の大きな
特徴の１つである。

実際にロボットの各関節を駆動させるモータＭを動作さ
せるときには、速度あるいは速度に相当する指令値をサ
ーボアンプに与えるため、速度ｖを計算手段で算出する
ことが有用であるから、上記（１）式は次の（２）式の
ように変形される。

更に、上記（２）式を離散値系で示すと、次の（３）式
及び（４）式となる。

ここで、Δｔはサンプリング時間、サフィックスｉまた
はｎはｉまたはｎ番目の値である。

上記の式から明らかなように、本発明による処理方法に
よれば、速度の計算に微分処理が含まれず、ノイズに対
して強く、誤動作がなくなり、ロボットのハンドに安定
した動きをさせることができる。

更に、離散値系に変形した上記（４）式の右辺にｖ
_ｎ−１（ｎの１つ前のｖ）があり、左辺にｖ_ｎがある。
つまり、ｎ番目のロボットのハンドの速度ｖを計算手段
で算出するために、１つ前の速度ｖさえあればよく、毎
回タコジェネTJ等から実際の速度ｖを検出し、フィード
バックする必要が無い。これは実際にロボットの駆動系
に取付けてあるタコジェネTG等の速度検出器を用いてロ
ボットのハンドの速度を検出することは、同じようにロ
ボットの駆動系に取付けてあるエンコーダPE等の位置検
出器で検出されるロボットのハンドの位置の検出に比較
して、信号の信頼度がやや低いことを考慮すると有用な
特徴である。

なお、（４）式において、位置ｘに関するものとしてｘ
_ｎ−１が唯１つしか無い事からも解かるように、ｘ位置
は毎回検出する必要があるが、この位置ｘは、エンコー
ダPEからのロボットの位置信号（関節角度）から算出
される。

ここで、m,k,Cはツールの特性を想定する際に決める値
である。ツールを想定したならば、そのツールの機構を
解析し、ロボットに動作させる前にこれらの値をオフラ
インで決定しておき、この値をそのツールに固有のデー
タ（定数であっても変数であっても良い。）として、操
作卓23から入力して制御装置17のRAM20にストックして
おく。つまり、想定したツールが複数であれば、それに
応じてストックすべきデータも複数となる。

また、上記（１）式で示される系が安定する条件は、常
微分方程式に関する基礎理論で既知である。それに基い
てm,c,kを定めれば安定させたり、発散させたりするこ
とも可能である。

これと同様に、上記（２）式を離散値系に変換した上記
（４）式で安定させるためには、上記の条件の他にΔｔ
の値も十分小さくする必要がある。

ただし、Δｔはサンプリングタイムであるから、正値を
取り、ｍは質量に相当する値であるから、正値を取る。
また、安定させる目的で用いるならばＣは正値とする。

以上まで所で、本発明により、ツールの動作特性をロボ
ットにさせる方法について述べたが、これは主に前記m,
C,kの値に基づく方法に関するものである。

本発明は、上記の方法以外にも別の変数であるロボット
に加えられる力Ｆ、この力の基準値Fc,ツール位置の基
準値xrに基づいてツールの特作特性を定めることができ
る特徴がある。

即ち、このF,Fc,xrの３値を指令値として用いた場合、
多くの組合せが考えられ、ツールに様々な動作特性を持
たせることができる。ここでは具体例として、以下の動
作例について述べる。

第４図は第１の動作例を示すもので、その条件は4mk−C
²＞0,C＞0,m＞0,xr＝0,Fc＝０であり、Ｆが第４図に示
す如くステップ入力Fs27を取ったとき、位置ｘは時間ｔ
に対して次の（６）式の関係となる。

ただしｘ_ｔ＝０＝0,v_ｔ＝０＝０とする。

この場合のロボットの動作は第４図に示す出力28とな
る。Fsステップ入力27の大きさに比例した値に位置が収
束し、その時の立上り時間や振動については、m,C,kで
決まる。

第５図は、第２の動作例を示すものでその条件は4mk−C
²＞0,C＞0,m＞0,F＝0,Fc＝０であり、xrに第５図に示す
ようにステップ入力ｘ_rs29を与えたとき、位置ｘは時間
ｔに対して次の（７）式の関係となる。

ただし、xt＝_０＝0,vt＝_０＝０とする。

この場合のロボットの動作は第５図に示す出力30とな
る。

これら第４図に示す出力28、第５図に示す出力30からわ
かるように、またこれらの出力と前記（１）式から推定
できるように、ある条件の下ではステップ入力をＦに与
えても、xrに与えても同等な出力が得られる。

即ち、同じ出力を得るのに変化させる値をＦにしたり、
xrにしたり、選択することができ、必要に応じて多種多
様の制御が可能となる。

第６図は、第３の動作例を示すもので、その条件はｋ＝
0,m＞0,C＞0,xr＝0,Fc＝０であり、第６図に示す如くＦ
にステップ入力Fs31を与えたとき速度ｖは時間ｔに対し
て次の（８）式の関係となる。

ただしｘ_ｔ＝０＝0,v_ｔ＝０＝０とする。

この場合、ロボットのハンドの速度は第６図に示す出力
32のようになり、上記の第1,第２の動作例と異なり、速
度が収束する。つまり、力によってロボットのハンドの
速度を制御することができる。

上記の如く簡単な３つの動作例を示したが、本発明はこ
の他にも有用な多数の制御を実行できる可能性を有す
る。

以上、１自由度のツールを想定し、ロボットを制御する
場合について、その原理や動作例について述べた。

しかし、実際にはツールが１自由度である場合は少な
く、多自由度のツールを使用されることが多い。

多自由度の場合は、ベクトルを用いて式（１）を一般化
することにより、次の式を得ることができる。

ただし：位置ベクトル（直交座標系でのロボットの並
進位置及び回転角度） ：速度ベクトル（直交座標系でのロボットの並進速度
及び回転角度） ：力センサ等から検出される力信号（並進力と回転力
を含む）のベクトル r:力基準値のベクトル c:位置基準値のベクトル 〔Ｍ〕：ツールの仮想質量を示すマトリックス 〔Ｃ〕：ツールの仮想粘性係数を示すマトリックス 〔Ｋ〕：ツールのバネ定係数を示すマトリックス である。ここで、〔Ｍ〕，〔Ｋ〕，〔Ｃ〕は一般には対
角行列になる。

式（９）を変形すると式（10）となる。

＝〔Ｍ〕^-1∫〔−ｒ）−（〔Ｃ〕＋〔Ｋ〕（
−ｒ））〕dt …（10） ここで式（10）による計算は、実際には計算機では次の
如く離散値係によって行なわれる。

ただしｎ＝1,2…… 式（11），（12）に於いて Δt:サンプリング時間を示すスカラ 〔Ｉ〕：単位マトリックス なお、式（12）において初期値_０は別途定める必要が
あり、また多自由度の場合はロボットの自由度と同じ
か、または、それより少ない自由度を持つ、ツールを想
定することが必要である。

以上、多自由度の場合についても述べたが、１自由度の
説明において触れたように、速度を計算する上で必要な
位置情報や速度情報をハード的に検出器から得る方法と
ソフト的に計算結果の値をその都度使用していく方法が
あり、それぞれ様々な組合せが考えられる。以下にその
例を示す。

第７図は位置帰還をエンコーダPEからの位置情報を用
い、速度換還を演算結果の値を用いる第１の方法を示
すブロック線図である。

つまり、式（10）の右辺に於いてはエンコーダによっ
て検出された値を用い、また式（10）の右辺において、
はタコジェネレータTGによって検出された値ではな
く、演算手段によって算出された値をそのまま用いる
方法である。

ここで、から への変換は〔Ｊ〕^-1なる行列（〔Ｊ〕はヤコビアン行列
と呼ばれる。）を用いて行なわれ、からへの変換は
ロボットの関節座標系から、直交座標系への座標変換に
よって行なわれる。

次に、第８図は位置帰還も、速度帰還も検出器からの情
報を用いる第２の方法を示すブロック線図である。

ここで、離散値系での値ｎは例えば次式で求まる。

ｎ＝Δｔｎ−１＋ｘ_ｎ−１ …………（13） ただしｎ＝1,2,…… 更に第９図は、位置帰還も、速度帰還も計算結果の値を
用いる第３の方法を示すブロック線図である。

つまり、この方法は位置、速度の両帰還を計算結果
の値を用いて帰還させるものである。

なお、ここでは式（12）に於いて初期値₀,_０は別途
定める必要がある。

以上、３つの方法について述べたが、各々次のような特
徴を持つ。

即ち、帰還の目的からすると上記第２の方法（第８図の
方法）の如く、位置も速度も実際の値を検出して帰還さ
せるのがよい。しかし現在の技術レベルから判断すると
ロボットに取付けられた位置の検出器PEはかなり良い精
度を持つが、速度の検出器TGはそれに比べて劣る。この
ため、両者を折衷した第１の方法（第７図の方法）が現
実的に最も優れている。

また第３の方法（第９図の方法）は、計算量が増加する
ものの、全く検出器を必要としない点に特徴を有する。

なお、ツールは最大に６自由度を有することからして前
記マトリックスは６×６のマトリックスとなる。しか
し、作業に応じてツールの自由度、寸法等が変わるので
そのツールに合わせて〔Ｋ〕，〔Ｃ〕，〔Ｍ〕^-1のマト
リックスの値を実験等によって求めておき、制御装置17
のROM21にツールや作業内容に対応させて各データに記
憶させておくのがよい。

また、ツールの寸法はベクトル量に関係するのでツール
や作業内容に合せてROM21の各テーブルに記憶させてお
く必要がある。

以上に説明したように本発明は、ロボットの制御によっ
てツールに様々なフレキシビリティを持たせ得ることが
大きな特徴である。

次に、本発明における角速度の算出過程をさらに具体的
に説明するが、その前にモデルとなるロボットの構造，
設定する各座標系及び記号について述べる。

第10図に示すように、ロボットの構造として剛体の空間
内の位置決めには通常６要素が必要であり、またその据
付面積に比べ可動範囲が広いことから６自由度の関節形
ロボットのものとした。この関節形ロボットは、各関節
の回転角度θ_ｉ（ｉ＝１〜６）及び各アーム長さd₁,
a₁₂,a₂₃,a₃₄,d₅（）で表わされ、次に述べる各座標系
が設定されている。まず、ロボットが据付けられた場所
に固定のベース座標系（以下Ｂ系という）、ロボットの
先端部に設けられたハンドに固定のハンド座標系（以下
Ｈ系という）、センサ部に固定のセンサ座標系（以下Ｓ
系という）、運動の回転モーメントの基準点と原点と
し、動作条件を設定したり、指令速度を算出したりする
補助座標系（以下Ａ系という）、バネの平衡位置・姿勢
により定まり、上記のＨ系に対応するリファレンス座標
系（以下RH系という）及びバネの平衡位置・姿勢により
定まり、上記Ａ系に対応するリファレンス座標系（以下
RA系という）の各座標系がある。

さらにハンドの位置及び姿勢を表わすため次の４個の３
次元ベクトルを定める。

:B系で表示したＨ系の原点 :B系で表示したＨ系のｘ軸方向の単位ベクトル :B系で表示したＨ系のｙ軸方向の単位ベクトル :B系で表示したＨ系のＺ軸方向の単位ベクトル 以上のロボットの構造，座標系及び記号を用いて、本発
明による の算出方法の一実施例を具体的に述べる。

第11図は、第３図のブロック線図をさらに詳細に示した
ものである。前述した様に、制御装置17は、位置・姿勢
算出部40、力算出部50、制御主要部60及び関節角速度算
出部70の４つの主なブロックに分けられる。

力算出部50は、さらに変換部51、座標変換部52、重力分
算出部53及び座標変換部54から構成される。ロボットの
ハンドに外力ａが加えられると、ロボットに設けられ
た力センサ16の感知部例えば歪ゲージはこれを感知し、
加えられた力及びモーメントを電圧としてアンプ29及
びA/D変換回路30を介して変換部51に出力する。変換部5
1は送られてきた電圧に電圧／力変換マトリックス
〔Ａ〕^＋を掛け合わせ、まずＳ系における力ｓを算出
する。この変換マトリックス〔Ａ〕^＋は、予め力センサ
16に各軸方向に基準荷重を与えてキャリブレーションを
行ない、＝〔Ａ〕ｓの関係を有するマトリックス
〔Ａ〕を求め、〔Ａ〕^＋＝（〔Ａ〕^Ｔ〔Ａ〕）^-1〔Ａ〕
^Ｔの式から得られるものである。

この様に変換マトリックス〔Ａ〕^＋は予め算出されて記
憶され、制御演算中は定数として扱われるものであり、
１つの力センサは１つの変換マトリックス〔Ａ〕^＋に対
応し、他の力センサに変える場合は、その力センサに対
応した変換マトリックス〔Ａ〕^＋を求め、対処すること
ができる。この算出された力ｓはＳ系における値であ
るので、これをＨ系に座標変換するため次の座標変換部
52に出力される。

即ち、座標変換部52は、Ｈ系におけるＳ系の原点位置及
び姿勢（各軸の単位ベクトル） をパラメータとして用い、Ｓ系の力ｓをＨ系の力_Ｈ
に変換する。この変換された力_Ｈは次の補正部54に出
力される。

一方、位置・姿勢算出部40において後述する様に算出さ
れたＢ系におけるハンドの位置・姿勢，（，，
）から重力分算出部53はハンド及びハンドが把持する
対象物の重量Ｗと重心位置をパラメータとして、ハン
ドや対象物自体の自重によって力センサ16に影響を与え
ている力、即ちハンドや対象物の重力分の力ｇを算出
し、補正部54に出力する。補正部54は、このｇと前述
の_Ｈからハンド及び対象物の重力による影響を取り除
いて、真にハンドに加わった力を、＝_Ｈ−_ｇの
式より求める。この算出された補正後の力はＨ系であ
り、速度算出の行なわれるＡ系に変換する必要があるの
で、座標変換部55において、Ｈ系におけるＡ系の原点位
置及び姿勢（各軸の単位ベクトル） をパラメータとして補正後の力はＡ系に座標変換さ
れ、次に制御主要部60に出力される。

なお、ハンドの動作制御の態様により速度算出をＡ系で
行なう必要はなくＨ系の場合は、当然にこの座標変換部
55における座標変換は不用である。

位置・姿勢算出部40は、演算部41、Ａ系の位置・姿勢算
出部42、RA系の位置・姿勢算出部43、Ａ系とRA系の位置
・姿勢偏差算出部44及び座標変換部45から構成される。

まず、演算部41は、モータＭに連結されたエンコーダPE
の位置情報からカウンタ28によって得られた位置信号
に基づいて、ロボットの各アーム長さとアーム先端部
に対するハンドの位置・姿勢_Ｅ，（_Ｅ，_Ｅ，
_Ｅ）をパラメータとしてＢ系におけるハンドの位置・
姿勢，（，，）を算出する、即ち の各式からそれぞれ，，，が求まる。

ここで、〔Ａ_1h〕は第１の座標系からＨ座標系への変換
行列を示し、，，_Ｅ，_Ｅ，_Ｅ，_Ｅの関数で
ある。算出されたハンドの位置・姿勢，（，，
）は前述した力算出部50内の重力分算出部53と後述す
る角速度演算部70とＡ系の位置・姿勢算出部42に出力さ
れる。RA系の位置・姿勢算出部43では、Ｈ系（ハンド）
に対応する並進バネ及び回転バネの平衡位置・姿勢r
h,（rh,rh,rh）かから を用いてＡ系に対応する。並進バネ及び回転バネの平衡
位置・姿勢ra,（ra,ra,ra）を算出する。

さらに、Ａ系とRA系の位置・姿勢偏差算出部44ではr
a,（ra,ra,）とa,（a,a,ａ）から位置の
偏差Δ及び姿勢の偏差，を次式により算出する。

ここでは、（ra,ra,ra）なる姿勢から、（a,
a,ａ）なる姿勢への回転を行なう軸方向の単位ベク
トルを、は前記軸回わりの回転角をそれぞれ意味す
る。

さらに、これらの演算結果に基づき、Ｂ系における位置
・姿勢偏差Δを次式により求める。

この式から明らかなように、偏差Δｘは、パラメータ
ra,（ra,ra,ra）の変更により変化させることが
できる。この算出された偏差Δｘは、座標変換部45にお
いてＢ系から速度算出の行なわれるＡ系に変換される。

なお、ハンドの動作制御の態様により速度算出をＨ系で
行なう場合は、偏差ΔｘはＨ系に変換されるが、ここで
は速度算出がＡ系で行なわれる場合について述べる。

次には動作条件設定部61、指令速度演算部62、及び座標
変換部63から構成される。

まず、動作条件設定部61において、Ａ系における様々な
ハンドの動作条件を加味してハンドに加わった外力＊
を算出することによりハンドに高度な運動を実行させる
ことができる。

なお、具体的な応用例は後述する。

指令速度演算部62は、力算出部50で算出された力（動
作条件設定部を経てとなる）及び位置・姿勢算出部40
で算出された位置・姿勢の偏差Δから力基準値ｃ及
び質量マトリックス〔Ｍ〕，粘性定数マトリックス
〔Ｃ〕，バネ定数マトリックス〔Ｋ〕をパラメータとし
てＡ系におけるハンドの指令速度を次式により算出す
る。

＝〔Ｍ〕^-1∫（−ｃ−〔Ｋ〕Δ〔Ｃ〕ｖ）dt…
…（19） 前述の原理的説明で述べた様に、本式によれば〔Ｍ〕，
〔Ｋ〕，〔Ｃ〕によって力を加えられたときのハンドの
挙動が定まるため、〔Ｍ〕，〔Ｋ〕，〔Ｃ〕の各パラメ
ータを適切に定めることによりハンドの動作特性を設定
することができる。

また、力基準値ｃを適切に定めることによっても押付
力を一定に保持したり変化させたり様々なハンドの動作
特性を得ることができる。

以上算出された指令速度はＡ系における値であるの
で、座標変換部63において前述したパラメータ を用いてＢ系に変換され、次の角速度演算部70に出力さ
れる。

実際にロボットは、各関節の角速度で駆動・制御される
ため、指令速度は角速度演算部70において、ロボット
の各関節の回転角度を示す位置信号、ロボットの各ア
ーム長さ及び前述した変換行列〔Ａ_1h〕をパラメータ
として用い、各関節の に変換される。この はD/A変換回路27及びサーボアンプ18を介して各アーム
駆動用のモータＭに入力され、ロボットのハンドは所定
の速度で動作することになる。

次に本発明による制御方法の具体的な応用例を次に述べ
る。

まず、第１の応用例はRA系が移動せずＢ系に固定であ
り、Ａ系をＨ系に一致させ従ってRA系とRHが一致し、Ａ
系の初期の位置・姿勢がRA系と一致した例である。

この場合のハンドの動作を第12図を用いて説明する。第
12図（ａ）に示すように、Ｈ系におけるハンド15にｙ軸
方向になる力が加わると、Ｈ系における外力は、 と示される。ここで、式（19）においてｃ＝〇とし とすると、外力を受けたハンド15は第11図（ｂ）に示
す位置・姿勢で静止する。ここで Δｙ＝ｆ_ｙ/h₂₂ …………（24） Δθｘ＝mx/h₄₄ …………（25） となり、本発明の制御方法によれば、パラメータ〔Ｋ〕
の適切な設定によってロボットの手首部にバネを付加し
たときと同等の動作を得ることを意味している。

即ち、力を瞬時に取り除くとハンドの原点Ｏ_Ｈは位置
の基準値ｒの原点即ちハンドの原点Ｏ_Ｈの初期位置を
中心として振幅Δｙなる単振動をし、またハンド15の姿
勢は第12図（ｂ）に示すように初期姿勢を中心として振
幅Δθｘとなる角度で示される回転の単振動をする。

つまり、ハンドの原点Ｏ_Ｈの振動とハンドの姿勢の振動
が合成された振動をハンド15は行なう、具体的には式
（６）で示す減衰になるのでハンド15の位置と は次の様になる。

つまり、ハンド15に任意の並進力・回転力が加わると、
ハンド15の先端点は初期位置へ、またハンド15の姿勢は
初期姿勢に戻ろうとする復元力と減衰力が働く。

よって、実際にはバネがないにもかかわらず、あたかも
ハンド15が空中に上記の条件を満たすような６自由度の
バネでつるされているような動きをすることになる。

次に、〔Ｋ〕＝〔〇〕とおくと、質的に異なった動作を
ハンドにさせることができる。この場合は、バネの力即
ち復元力が無いのでハンドは元の位置や姿勢に戻ろうと
することはなく、Ｈ系に関して加えられた並進力の方向
に並進し、回転力の中心となる軸回りに回転する。つま
り空中間において、加えられた並進力及び回転力の方
向、大きさでなすがままに動き、力を加えることをやめ
れば静止するという運動が実現できる。

例えば、第12図（ａ）に示すような力を加えると、こ
の時の速度は式（８）より となる。

これまでの説明では、バネ定数マトリックス〔Ｋ〕の６
個の対角要素が全て〇でない全ての場合を考えたが、こ
の６個の対角要素の一部を〇とし、一部を〇としないよ
うに設定すると、例えば、力を加えるとハンドの姿勢に
関しては復元力が働き、常に元の姿勢に戻ろうとするが
位置に関しては押されるがままに動くという上述した２
つの動作特性とは異なった動作特性をハンドに持たせる
ことができる。

次に第２の応用例を述べる。

第２の応用例はRA系が移動する場合であり、Ａ系はＨ系
に一致させ、従ってRA系とRH系が一致し、RA系が移動す
る例である。

この場合のハンドの動作を第13図を用いて説明する。

前述した第１の応用例では、RA系がＢ系に固定であっ
た。このため、ハンドにはRA系が示すところの位置・姿
勢への復元力が働き、また復元力はRA系からの偏差に比
例していたために、実際にはハンドはRA系の近傍でしか
作業することはできない。

しかし本例では、このRA系の位置・姿勢をＢ系上で移動
するところに特徴がある。RA系を移動させるとは、復元
力の平衡位置・姿勢を移動させることであるので、ハン
ドがその移動に追従する形で移動する。

例えば第13図に示すごとく、RA系の位置・姿勢を移動さ
せると、これに追従してハンドの位置・姿勢も移動し、
同時に前述した第１の応用例で示した動作特性も合わせ
持つ。

即ち、ハンドの移動中に力が加われば、RA系の移動径路
をその力に応じて外れるが、その瞬間に同時にRA系への
復元力が働くので、また移動径路に戻る。

つまり、手首部にバネが付加された機構を持つロボット
に於て、そのハンドが移動しているような運動を制御の
パラメータを与えるだけで実現できる。このときの動作
は、式（７）で示されるが、これをRA系が静止している
ときに力を加えたときの動作を示す式（６）と比較する
と基本的には同一であることがかわる。これは、ロボッ
トに加える力（RA系が静止時）とRA系の移動量を適切に
定めれば、この両者を置き換えてもハンドが同一の動作
を行い得ることを示唆するものである。

第３の応用例として、ハンドの動作条件を設定した場合
の例を示す。

最初の例は、動作条件を回転速度のみとし、Ａ系はＨ系
に一致させず、従ってRA系はRH系に一致せず、RA系はＢ
系に固定で、Ａ系の初期位置・姿勢と一致させるもので
ある。

この場合のハンドの動作を第14図を用いて説明する。

ハンド15が持つシャフト80の先端点をＡ系の原点Ｏ_Ａと
する。基本的な動作は、第12図で示したものと同一であ
るが、Ａ系はＨ系と違う位置にしてある点が異なる。

また動作の拘束条件として、並進力及びＺ軸回りの回転
力をＯとした。

即ち、Ａ系での並進力をＯとし、従ってＡ系の原点Ｏ_Ａ
回りの回転力のみ許される。但しＺ軸回りの回転力はＯ
とした。このため力センサが検出できる範囲内で、ハン
ド及びシャフトのどの点にどのような方向に力（ただし
作用線が原点を通過しないものとする）を働かせても、
ハンド及びシャフトはｘ軸回りとｙ軸回りとの合成され
た回転運動しか行なわない。そして力を除けば元の姿勢
に復帰する。

この動作は前述したRCC機構の本質をなすシャフト（ワ
ーク）の先端点回りの回転を実現するものであり、自動
組立、特に精密嵌合作業に有用な動作である。

次の例はハンドの動作条件を並進力のみとし、Ａ系はＨ
系に一致させ、従ってRA系はRH系に一致し、かつRA系は
Ｂ系に固定でＡ系の初期位置・姿勢と一致させるもので
ある。

この場合のハンドの動作を第15図を用いて説明する。

基本的な動作は第12図に示したものと同じであるが、動
作の拘束条件として、回転力をＯとした。

つまり、Ａ系での回転力をＯとした。従って並進運動の
み許される。このため、どのような力をどの点に加えて
もハンドは姿勢が一定のまま、並進運動をする。そして
力を除けば元の位置に復帰する。

また、式（23）に於いて、h₁₁,h₂₂を大きく、h₃₃を小さ
く定めた。

つまり、Ｈ系のｘ軸,y軸方向には柔いバネとして働き、
Ｚ方向には硬いバネとして働く。このため第15図で示す
ように、同じ力を加えれば、Ｚ軸方向に大きく振れ、xy
平面上では振れは小さい。

またここでは、Ａ系をＨ系に固定にしたので第15図で示
したように、この性質（Ｚ軸方向に大きく振れる）は、
ハンドの姿勢が変わっても常にハンドの中心軸方向（即
ちＨ系のＺ軸方向）に大きく振れることに変わりはな
い。

最後の例は、同じくハンドの動作条件を並進条件のみと
し、Ａ系の原点はＨ系に一致させるが、その軸方向はＢ
系に一致させるものである。

この場合のハンドの動作を第16図を用いて説明する。

この例においてはＡ系の方向がＨ系と異るため、ハンド
の姿勢が変われば大きく振れる方向は、ハンド系から見
て変わる。ただし、Ｂ系から見て不変である。つまり大
きく振れる方向は常にＢ系のＺ軸方向となる。

従って上記２例によって、ハンドの動作条件をＨ系にお
いてでも、Ｂ系においてでもＡ系の定め方一つで変えら
れることがわかる。

次に、本発明による制御方法に非接触センサからの情報
を取り入れた応用例を述べる。

第17図はロボットをある作業領域空間内にて位置制御し
ている最中に、万一危険領域内に入るような場合には危
険領域内への侵入を非接触センサによって回避せんとす
るものである。既述のように、ロボットの位置制御は位
置基準値raを操作することによって行なわれるが、こ
れよりすればraを作業領域空間内にとれば、如何に操
作しようともロボットはほぼ作業領域空間内に留まるこ
とが予想される。

しかしながら、実際には設定されたパラメータ〔Ｍ〕，
〔Ｃ〕，〔Ｋ〕やロボット駆動系の遅れなどによって、
あるいは誤動作や外乱などによってロボットが作業領域
外、即ち、危険領域内に入ることがあり得るというもの
である。第17図に示す例ではｘ軸の正、負方向に壁116,
117があって、壁116,117からの距離がΔlc以内の領域が
危険領域として設定されている。したがって、ロボット
としてはｘ軸の正方向、負方向における壁117,116との
間の距離を測定すべく少なくとも２個の非接触センサ11
8を必要とし、これら非接触センサ118によってロボット
ハンドが危険領域の何れかに侵入したことが知れるもの
である。

例えば、ロボットハンドが壁117に近づきつつある場合
を想定すれば、壁117との距離Δlx＋がΔlx＋＜Δlcと
なったことを以て危険領域に侵入したことが知れるもの
である。もしも危険領域にロボットのハンド15が侵入し
た場合にはΔlx＋がΔlcとなるべくロボットのハンド15
を押し戻すことが必要である。

第18図に示すように、ある時点tcで危険領域に侵入した
ことが検知された場合には、直ちに第５図に示した如く
の挙動をとらせ円滑に危険領域より脱しさせるものであ
る。勿論従来より行なわれているように、その時点tcで
非常停止せしめたり、あるいは円滑にではなく直ちに大
きな加速度を以て危険領域を脱しさせることも可能であ
る。

ところで、非接触センサによる上記のような制御はこれ
までに既に述べた本発明による制御方法に非接触センサ
からのフイードバック信号を加味することによって容易
に達成可能である。

また、以上のような制御以外に第19図に示すような制御
も考えられている。第19図（ａ），（ｂ）はロボットが
人間に近づきその間の距離がΔlc,x＋よりも小さくなっ
た場合は、ロボットが人間から離れΔlc,x＋の位置まで
戻るべく制御され、また、それとは逆に人間がロボット
に近づいた場合も同様に制御されることが示されてい
る。第19図に示す制御はいわば壁が移動可とされた場合
に相当するものであり、このような制御によって非接触
直接教示が可能となるものである。

以上２つの応用例は何れもロボットと対象物との間の距
離を検出したうえ、ある基準値との関係にもとづいてロ
ボットを制御するようにしたものである。このような制
御を拡張したものとして第20図に示す如くのものが考え
られている。第20図に示すものにおいては非接触センサ
118以外に接触センサとしての力センサ16もロボットに
装着され、ハイブリットな制御が可能となっている。

例えば軸状ワーク119を穴が穿たれているワーク120に組
付する場合を想定すれば、ロボットはワーク119把持し
た後はワーク120近傍まで搬送することが必要である。
ワーク120への搬送は高速に行なわれることが望ましい
が、高速搬送の場合はワーク120に接触した時点で大き
な衝撃を受けることから、接触直前に搬送速度を小さく
することが必要となる。一般に接触センサのみしか装着
されていない場合は、大きな衝撃で初めて接触が検知さ
れるか、または大きな衝撃を回避すべくワークは低速に
て搬送されることになるが、何れによるとしても大きな
衝撃または低速といった不具合を生じることになる。

しかしながら、非接触センサを併用する場合には大きな
衝撃を回避し得、しかも高速にワークを搬送することが
可能となるものである。

即ち、ワーク120にワーク119が接触する直前を非接触セ
ンサ118によって検出したうえで搬送速度を小さくし、
ワーク119が低速搬送の状態でワーク120に接触した後は
接触センサ118によってワーク119のワーク120への組付
を行なわんとするものである。

以上説明した応用例は、非接触センサによってロボット
ハンドと障害物などの対象物との間の距離が一定以内で
あることが検出された場合にはその検出時点でロボット
に対する制御モードを変更するようにしたものである。

したがって、対象物との関係でロボットは適切に制御さ
れ、更に接触センサを併用する場合はそれとの協調制御
も可能となっており、多種の制御動作特性が制御上での
パラメータを変更するだけで容易に実現し得るなどの効
果がある。

〔発明の効果〕

以上に説明したように本発明によれば、ロボットの制御
によってフレキシビリティを持たせることが可能とな
り、次のような作用効果を有する。

即ち、必要とする動作を行なうツールを想定し、そのツ
ールと同等の動作をロボットにさせることができ、かつ
想定するツールを変換できることを実現することであ
る。

従来は、遂次ツールの交換をしていたが、本発明によれ
ば、この交換を単なる制御上のバラメータの交換ででき
るようになった。

また、同一のツールを用いてはいるが、若干特性を変え
た動作をロボットにさせる時にも、従来ならば、遂次ツ
ールの一部を調整しないとできなかったが、本発明によ
ればオンラインでツールの特性を必要に応じて制御の面
だけで変換できるという便利な機能を持たせることがで
きる。

以上の如く一貫して、本発明をツール代替機能の面から
述べてきたが、本発明によればこの他にシミュレーショ
ン機能を持たせることができる。

これは、例えばロボットに取付ける手首機構を設計する
に際し、実際に物を作る前にその機構の挙動を見たい時
に本発明の手法を適用してロボットにその動作を実際に
させることができる。

上記のツール代替、シミュレーションの各機能は、ロボ
ットの精度が高い程、その効果も高い。しかし、現実に
はロボットはアーム部の弾性や減速歯車の剛性不足等に
よる振動他の機械的な原因やまた電気回路その他の原因
で、不要な振動、遅れ等が生じている場合がある。

このような時に、ロボットのこの振動、遅れ等を解析し
て予想できたならば、それを少くするための補償動作を
本発明によれば逆に加えることも可能となる。

以上述べた様に、本発明を有用な様々な効果を奏するも
のである。

【図面の簡単な説明】 第１図は本発明のロボット制御方法の全体構成を示す構
成概略図、第２図は本発明のロボット制御装置の構成要
素を示す構成図、第３図は本発明のロボット制御装置を
示す概略ブロック線図、第４図は本発明の制御方法によ
るロボットの第１の動作例を示す説明図、第５図は本発
明の制御方法によるロボットの第２の動作例を示す説明
図、第６図は本発明の制御方法によるロボットの第３の
動作例を示す説明図、第７図は本発明の制御方法におけ
る演算フローの第１の実施例を示すブロック線図、第８
図は本発明の制御方法における演算フローの第２の実施
例を示すブロック線図、第９図は本発明の制御方法にお
ける演算フローの第３の実施例を示すブロック線図、第
10図はロボットの構成及び座標系を示す説明図、第11図
は本発明のロボット制御装置を示す詳細なブロック線
図、第12図（ａ），（ｂ）は本発明の制御方法の具体的
な第１の応用例を示す説明図、第13図は本発明の制御方
法の具体的な第２の応用例を示す説明図、第14図，第15
図，第16図は本発明の制御方法の具体的な第３の応用例
を示す説明図、第17図，第18図，第19図，第20図は本発
明による制御方法と非接触センサを組合せた実施例を示
す説明図である。 10……（関節形）ロボット、15……ハンド、16……力セ
ンサ、17……制御装置、18……サーボアンプ、40……位
置・姿勢算出部、50……力算出部、60……制御主要部、
70……関節角速度算出部。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 坂上 志之 神奈川県横浜市戸塚区吉田町292番地 株 式会社日立製作所生産技術研究所内 (56)参考文献 特開 昭60−254209（ＪＰ，Ａ) 特開 昭60−205716（ＪＰ，Ａ)

Claims (4)

【特許請求の範囲】
1. 【請求項１】サンプリングタイム毎にロボットを制御す
   るロボットの制御方法であって、 前サンプリングタイム（ｎ−１）におけるロボットのハ
   ンドに加えられた実際の力を検知して力信号を算出し、
   この信号と前サンプリングタイム（ｎ−１）における目
   標値であった力基準値との差である第１の値を算出し、 前記ハンドの前サンプリングタイム（ｎ−１）の実際の
   位置及び姿勢を表わす位置信号と前サンプリングタイム
   （ｎ−１）における位置及び姿勢の目標値であった位置
   基準値との差にバネ定数を掛けた第２の値を算出し、 前記ハンドの前サンプリングタイム（ｎ−１）における
   速度信号としてロボットに設けられた位置検出器からの
   位置検出値を微分した値を用い、これに粘性定数を掛け
   た第３の値を算出し、 前記第１、第２、第３の値と前記ハンドの質量係数から
   前記ハンドの次サンプリングタイム（ｎ）の指令速度を
   算出することを特徴とするロボットの制御方法。
2. 【請求項２】前記位置信号として、前記ロボットに設け
   られた位置検出器からの位置検出値を使用することを特
   徴とする特許請求の範囲第１項記載のロボット制御方
   法。
3. 【請求項３】前記位置検出信号として、前記ロボットに
   設けられた非接触センサからの位置情報を使用すること
   を特徴とする特許請求の範囲第１項記載のロボットの制
   御方法。
4. 【請求項４】サンプリングタイム毎にロボットを制御す
   るロボットの制御装置であって、 前サンプリングタイム（ｎ−１）におけるロボットのハ
   ンドに加えられた実際の力を検知して力信号を算出し、
   この信号と前サンプリングタイム（ｎ−１）における目
   標値であった力基準値との差である第１の値を算出する
   算出手段と、 前記ハンドの前サンプリングタイム（ｎ−１）の実際の
   位置及び姿勢を表わす位置信号と前サンプリングタイム
   （ｎ−１）における位置及び姿勢の目標値であった位置
   基準値との差にバネ定数を掛けた第２の値を算出する算
   出手段と、 前記ハンドの前サンプリングタイム（ｎ−１）における
   速度信号としてロボットに設けられた位置検出器からの
   位置検出値を微分した値を用い、これに粘性定数を掛け
   た第３の値を算出する算出手段と、 前記第１、第２、第３の値と前記ハンドの質量係数から
   前記ハンドの次サンプリングタイム（ｎ）の指令速度を
   算出する算出手段とを備えたことを特徴とするロボット
   の制御装置。