JPH0146275B2 - - Google Patents

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は、工業用ロボツト等の作業機械のなら
い制御方法および装置に係り、特に、ならい動作
の対象物の曲率が変化しても常に円滑なならい動
作を行うに好適なならい制御方法および装置に関
する。

〔従来の技術〕

従来から行われているならい制御方式は、なら
い動作の基本となる動作方向（以下、「基本動作
方向」という）を常に一定に保ち、これと直角な
方向の対象物の凹凸をセンサにより検出し、その
凹凸の形状に応じて作業機械に取付けられた作業
工具またはセンサを上記センサの検出方向に駆動
し、対象物形状にならわせる方式である。この場
合、対象物の形状の接線方向がセンサの検出方向
と一致するかほぼ一致する個所においては、なら
い動作が不可能となる欠点があつた。また、この
方式では、対象物に対する作業工具やセンサの姿
勢と対象物に沿つた動作速度とを一定に保つこと
が困難であるという欠点かあつた。この欠点は、
例えば塗装ロボツトでは、塗装むらを生じる原因
となる。

これらの欠点を解消する方策として、出願人は
既に特開昭55−18376号および特開昭58−34781号
に記載の方法を提案している。これらの方法で
は、センサの検出結果として得られる対象物の接
線方向情報に着目し、ならい動作の基本動作方向
を上記接線方向に一致させるように逐時変更する
という新しい考え方が導入されている。

〔発明が解決しようとする課題〕

しかし、この方式は、対象物の接線方向の変化
量すなわち対象物表面の曲率を一定タイミングま
たはならい動作移動距離ごとに検出し、その検出
ごとにならい動作の方向を変更するものであつ
て、対象物表面の曲率そのものまでも考慮したも
のではなかつた。言い換えると、上記提案の方法
は、対象物表面の曲率にかかわらず、一定長さ
ΔLの線分で対象物表面形状を折れ線近似するこ
とに相当するならい制御方法であつた。

ところが、対象物表面の形状を折れ線近似する
にあたり常に同一長さの線分を用いると、対象物
表面の曲率半径が小さい個所すなわち対象物表面
の接線方向の変化が激しい個所においては、なら
い動作方向と実際の接線方向との誤差が大きくな
るという問題が依然として残つている。

また、このように対象物表面の接線方向の変化
が激しい部分においてもゆるやかな部分において
も、一定移動距離ごとに曲率を検出すると、はな
はだしい場合はならい動作中に対象物がセンサの
検出範囲を外れてしまい、ならい動作の継続に必
要なセンサ出力が得られなくなつたり、センサと
対象物との干渉のおそれがあるなど大きな問題が
残つていた。

本発明の目的は、対象物表面の接線方向の変化
量すなわち対象物表面の曲率がならい動作中に大
きく変化しても、これに適切に対処しながらなら
い動作を円滑に継続できるならい制御方法および
装置を提供することである。

［課題を解決するための手段］ 本発明は、上記目的を達成するために、移動体
の先端に取付けられ対象物との相対距離を検出す
るセンサを対象物近傍で指定面内の接線の指定方
向に設定移動距離だけ移動させ、センサと対象物
との相対距離および移動体の３次元空間内の位置
に基づき対象物の位置およびこの対象物の位置に
おける指定面内の接線を演算し、その接線の指定
方向の延長上で前記設定移動距離だけ離れた点に
次の動作目標点を設定し、センサをその動作目標
点に向かつて移動させるならい制御方法におい
て、前記接線方向の変化量に応じて前記設定移動
距離を変更するならい制御方法を提案するもので
ある。

前記指定面と交わる面内に前記センサを複数個
設け、これらセンサの出力に基づき３次元形状の
対象物の前記交わる面内における法線方向を演算
し、前記３次元形状の対象物上の点における接平
面の法線を求め、この法線に対して一定の角度以
内となるように前記センサの方向を規制すること
もできる。

前記センサは、前記接線に対する法線方向に正
確に向いている必要はなく、距離測定に所定値以
上の誤差を招かない範囲で一定の角度を保ちつつ
移動するようにしてもよい。

本発明は、また、移動体の先端に取付けられ対
象物との相対距離を検出するセンサを対象物近傍
で接線方向に設定移動距離だけ移動させる駆動手
段と、前記センサと対象物との相対距離および移
動体の３次元空間内の位置に基づき対象物の位置
およびこの対象物の位置における指定面内の接線
を求め前記駆動手段に動作目標点を出力する第１
演算手段と、前記移動前後の位置における接線方
向の変化量に応じて前記設定移動距離を変更し前
記第１演算手段に出力する第２演算手段とを含む
ならい制御装置を提案するものである。

［作用］ 本発明においては、センサを対象物表面の接線
方向に設定移動距離だけ移動させる際に、移動前
後の接線方向の変化量が所定値を超えて増加する
に従い、前記動作目標点を設定する距離を前記設
定移動距離よりも短く変更するので、接線の方向
が徐々に変わる比較的なだらかな対象物表面は勿
論のこと、接線の方向の変化量が大きいすなわち
曲率半径が小さいコーナ部等においても、常に安
定して正確なならい制御が可能となる。

また、このならい動作の指定面と交わる（例え
ば直交する）面内に複数のセンサを配置し、これ
らセンサの出力に基づき３次元形状の対象物の前
記交わる面内における法線方向を演算し、前記３
次元形状の対象物上の点における接平面の法線を
求め、この法線に対して一定の角度以内となるよ
うに前記センサの方向を規制すると、前記交わる
面方向にも曲面となつている３次元対象物のなら
い動作がより正確になされる。

〔実施例〕

以下、図面を参照して本発明の実施例を具体的
に説明する。

第１図は、本発明によるならい制御装置の一実
施例を備えた作業機械として、曲面Ｗの形状を計
測するロボツト１を示す斜視図である。ロボツト
１は、油圧シリンダ２により第１コラム３の上を
水平方向（以下「Ｘ軸方向」という）に往復移動
する第１移動体４と、第１移動体４上に直立に設
けられた第２コラム５に沿つて油圧シリンダ６に
より上下方向（以下「Ｚ軸方向」という）に移動
する第２移動体７と、第２移動体７に取付けられ
油圧シリンダ８によりＸ軸と直交する水平方向
（以下「Ｙ軸方向」という）に往復移動する第３
移動体９と、第３移動体９の先端に取り付けられ
た手首機構１０とからなる。

手首機構１０は第３移動体９に固定された垂直
軸１１のまわりに回転する振りブロツク１２と、
振りブロツク１２に取付けられ水平軸１３のまわ
りに回転する曲げブロツク１４とからなる。

曲げブロツク１４には、作業対象物との近接距
離を検出する渦電流式センサ２１が取付けられ、
その主軸線２１０が水平軸１３と常に直交するよ
うになつている。曲げブロツク１４にはまた、セ
ンサ２１と同一型式のセンサ２２がセンサ２１と
平行に軸間距離Lsをもつて配置されている。セ
ンサ２２は、センサ２１の主軸線２１０が水平と
なるように曲げブロツク１４を駆動して位置決め
した場合、センサ２１の上方に位置するように配
置されている。

第２図は、第１図のならい制御装置およびロボ
ツト１の制御系のブロツク図である。マイクロコ
ンピユータ等からなる制御用計算機５１からの位
置指令は、インターフエース５２においてロボツ
ト１の各軸にふり分けられ、各軸の位置決め装置
５３〜５７に送り出される。例えばＸ軸の位置決
め装置５３に送られた位置指令は、減算回路５３
１が位置検出器５３４から得られたロボツト１の
Ｘ軸の現在位置との差を算出する際に用いられ、
その算出結果はサーボ回路５３２に送られる。サ
ーボ回路５３２の出力は、油圧サーボ弁や油圧シ
リンダ２などからなるロボツトＸ軸機構５３３を
駆動させる。Ｘ軸機構５３３の位置は位置検出器
５３４により検出され、減算回路５３１に送られ
ると共に、計算機５１にも読み込まれる。

また、センサ２１，２２の信号はセンサインタ
ーフエース５８により線形化して増幅され、計算
機５１に読み込まれる。

次に、第３図〜第１０図を用いて、本発明のな
らい制御装置のならい制御アルゴリズムを説明す
る。ここでは説明を容易にするために、ならい動
作は水平面内で行われるものとし、第３図〜第５
図においては水平面内での制御方法の原理を、第
６図〜第１０図においては垂直平面内での制御方
法の原理を示しているが、ならい動作は水平面内
に限定されるものではなく、任意の平面内更には
曲面内において実行できるということはいうまで
もない。

なお、以下の説明では、ロボツト１の手先に取
付けられたセンサ２１の主軸２１０上で、センサ
２１の先端から距離Ssだけ離れた点Ｐをロボツ
トの手先位置とし、この手先位置に着目して動作
を説明する。また、センサ２１の検出可能な近接
距離の最小値をS_nioとし同じく最大値をS_naxとす
れば、S_nio＜Ss＜S_naxなるように距離Ssを選び、
この距離Ssをセンサ２１の標準検出距離と呼ぶ
ものとする。通常、距離SsはS_nioとS_naxとの平均
値をとればよい。

ロボツト１の手先位置Ｐの座標（X_P、Y_P、Z_P）
を、ロボツト１の基台に固定された座標系により
表わせば、Ｘ、Ｙ、Ｚ軸の移動量をｘ、ｙ、ｚ、
また、Ｙ軸方向から測つた手首振りブロツク１２
の回転量をα、水平面から測つた手首曲げブロツ
ク１４の回転量をβとし、垂直軸１１と水平軸１
３の軸間距離をl_A、水平軸１３からセンサ２１先
端までの距離をl_Pとすると、 X_P＝ｘ−｛l_A＋（l_P＋Ss） cosβ｝sinα Y_P＝ｙ＋｛l_A＋（l_P＋Ss） cosβ｝cosα］ Z_P＝ｚ＋（l_P＋Ss）sinβ ……(1) と表わすことができる。

まず、第３図〜第５図により水平面内でのなら
い制御について説明する。ロボツト１は最初にワ
ークＷのならい開始点Q₀の近傍に位置決めされ
ている。このとき、操作者が点Q₀におけるワー
クＷの接線とほぼ直角方向となるように、センサ
２１を予め位置決めしておく。ロボツト１はここ
から動作を開始し、センサ２１の出力がSsとな
つたところで一旦動作を停止する。このときのロ
ボツト１の手先位置ＰはQ₀点と一致している。

ならい動作は、センサ２１の主軸線２１０と直
角方向すなわちワークＷのおおよその接線方向に
ロボツト１を駆動することにより、開始される。
動作を開始し、移動距離L₀となつた時のロボツ
トの手先点をR₁とすると、ロボツトの手先がR₁
点に到達した時点で再度センサ２１のデータを入
力する。このときの測定距離をS₁とすれば、ワー
クＷ上の点Q₁の座標（X₁、Y₁、Z₁）は、ロボツ
ト各軸の移動量x₁、y₁、z₁、α₁、β₁を用いて X₁＝x₁−｛l_A＋（l_P＋S₁） cosβ₁｝sinα₁ Y₁＝y₁＋｛l_A＋（l_P＋S₁） cosβ₁｝cosβ₁］ Z₁＝z₁＋（l_P＋S₁）sinβ₁ ……(2) として求められる。

なお、以下では上記移動距離L₀を基準サンプ
リング距離と呼ぶことにする。また、点Q₀から
点R₁への基準サンプリング距離L₀分のロボツト
１の動きは、これらの点間を結ぶ直線上に微細な
距離ごとの中間点を設け、これらの点の座標に対
応するロボツト１各軸の位置を順次指令し、直線
上を手先Ｐが一定速度で移動するように制御す
る。以下の説明においても、ロボツト１の動きま
たはロボツト１を駆動するという表現は、上記の
ような直線上の一定速度での動作またはそれらの
組合わせ動作を意味する。

さて、ロボツト１が点R₁に到着した時点で、
(2)式によりワークＷ上の点Q₁が明らかとなる。
そこでワークＷの近似的な接線が点Q₀と点Q₁と
を結ぶ直線として決まる。なお、このときの接線
の傾きは点Q₀、点Q₁の座標から容易に求められ
る。

いま、ロボツト１のＸ軸正方向から測つた角度
を用いて接線の傾きをθ₁と表わせば、Q₀点の座
標を（X₀、Y₀、Z₀）として、角度θ₁は次のよう
になる。

θ₁＝tan^-1Y₁−Y₀／X₁−X₀；X₀≠X₁のとき ＝０ ；X₀＝X₁のとき 本発明の制御方式は、このようにして得られた
接線の延長上にならい動作の目標点を設定しなが
ら動作していくことが基本となつている。すなわ
ち、点Q₀、点Q₁とを結ぶ直線の延長上に、点Q₁
から距離L₀の点R₂を定め、これを目標として次
のサンプリング区間のならい動作が行われる。

なお、この場合、センサ２１によるワークＷの
位置測定のために、センサ２１が点R₂において
直線Q₀R₂と直角になるように駆動する。ここで
は、手首振りブロツク１２の回転角αを用いてセ
ンサ２１の姿勢を示し、例えばR₂点における姿
勢をα₂と表す。

ロボツトの手先が点R₂に到達した時点でセン
サ２１の出力を測定し、これをS₂とすれば、(2)式
と同様にR₂点におけるロボツト１の各軸の位置
をx₂、y₂、z₂、α₂、β₂とすると、ワークＷ上の点
Q₂の座標（X₂、Y₂、Z₂）は、 X₂＝x₂−｛l_A＋（l_P＋S₂） cosβ₂｝sinα₂ Y₂＝y₂＋｛l_A＋（l_P＋S₂） cosβ₂｝cosβ₂］ Z₂＝z₂＋（l_P＋S₂）sinβ₂ ……(3) と表せられる。直線Q₁Q₂の傾きθ₂は θ₂＝tan^-1Y₂−Y₁／X₂−X₁；X₂≠X₁のとき ＝０ ；X₂＝X₁のとき である。

近似的な接線は直線Q₀Q₁から直線Q₁Q₂に変化
し、その傾きはθ₁からθ₂となつた。この傾きの変
化は、実はワーク表面形状の曲率を表わしてい
る。すなわち、接線方向の変化量は曲率半径に対
応している。

本発明のならい動作は、ワーク表面の曲線を接
線により折れ線近似している。したがつて、同一
の精度で曲線を折れ線近似するためには、曲線半
径が小さいところでは、折れ線の長さを短くする
必要がある。すなわち、近似的なに得られた接線
の延長上に、サンプリング距離Ｌだけ離れた点を
ならい動作の動作目標点とするが、このサンプリ
ング距離Ｌを上記接線の傾きの変化量に応じて変
更する必要がある。

ここでは、接線の傾きの変化θ₂−θ₁の値に応じ
て、上記サンプリング距離Ｌを次のように変化さ
せる。

Δθ＝θ₂−θ₁ とし、サンプリング距離Ｌを Ｌ＝ΔS₀／tanΔθ；｜Δθ｜ ≧Δθcのとき Ｌ＝L₀ ；｜Δθ｜＜Δθcのとき ……(4) とする。ここで、ΔS₀、L₀、Δθcは定数であり、
L₀は基準サンプリング距離、ΔS₀はSs−Sminと
Smax−Ssのうちの小さい方よりも小さい値とす
る。一般的には(4)の連続性から、ΔS₀＝
L₀tanΔθcとする方が良い。また、Δθcはならい
精度の所要値から定める。

このようにして次のサンプリング距離Ｌを決
め、ならい目標点を定める。目標点R₃の決定は
(4)式により求めたＬをL₃とし、直線Q₁Q₂の延長
上にQ₂から距離L₃だけ離れた点として決定する。
点R₃にロボツト１が到達すれば、点R₃において
センサ２１の出力からワーク上の点Q₃の位置を
求め、今度は点Q₂とQ₃を結ぶ直線の延長上に次
の目標点R₄を定める。直線Q₂Q₃の傾きをθ₃とす
れば、R₄点における目標センサ姿勢を直線Q₂Q₃
に直角な姿勢とするための手首振りブロツク１２
の回転角α₄が得られる。このような処理を繰返し
ながら、ならい動作を行うのが、本発明の制御方
法の特徴である。

これを一般化すると、第４図のようになる。い
ま、点R_i+1にロボツト１の手先が到達している時
点において、直線Q_i-1Q_iの傾きをθ_iとし、このと
きのセンサ姿勢をα_i+1とする。点R_i+1においてワ
ークＷ上の位置を測定し、センサ２１の出力S_i+1
からワークＷ上の点Q_i+1の座標を求め、直線Q_i
Q_i+1の傾きθ_i+1を決定する。これより、 Δθ_i+1＝θ_i+1−θ_i が得られる。一方、点Q₁、R_i+1間の距離すなわち
前回のサンプリング距離をL_i+1とすれば、L_i+1の
距離間に接線の傾きがΔθ_i+1変化したから、基準
サンプリング距離L₀間の接線の傾き変化Δθ^* _i+1は、
曲率半径を一定と仮定して近似的に Δθ^* _i+1＝2sin^-1（L₀／L_i+1sinΔθ_i+1／２） ……(5) となる。そこで、次のサンプリング距離L_i+2を、
(4)式にならつて L_i+2＝ΔS₀／tanΔθ^* _i+1；｜Δθ^* _i+1｜≧Δθcのとき ……(6) L_i+2＝L₀ ；｜Δθ^* _i+1｜＜Δθcのとき として、次の区間のならい動作目標点R_i+2を直線
Q_iQ_i+1の延長上、点Q_i+1から距離L_i+2だけ離れた
点に設定し、かつR_i+2においてセンサ姿勢が直線
Q_iQ_i+1と直角な姿勢となるような角度α_i+2に設定
し、ならい動作を続ける。(6)式の関係を第５図に
示す。

次に、第６図〜第１０図により、本発明の垂直
面内での姿勢制御について説明する。まず、第６
図に示すように、ロボツト１はワークＷのならい
開始点Q₀の近傍に位置決めされており、センサ
２１，２２は、点Q₀におけるワークＷの垂直面
内での接線とほぼ直角方向となるように、操作者
により予めセツトされる。ロボツト１はここから
動作を開始し、センサ２１，２２の出力がともに
一致するように姿勢を制御して、一旦動作を停止
する。なお、姿勢制御にあたつては、ロボツト１
の手先Ｐ点の位置が変化しないように(1)式に基づ
いてロボツト１の各軸を駆動する。また、このと
きのセンサ２１の主軸２１１の傾きを水平面から
の仰角βで表わしてこれをβ₀とし、ワーク表面の
接線の傾きを鉛直面からの角度φで表わしてQ₀
点での傾きをφ₀とする。このときはφ₀＝β₀であ
る。ここでは、前述した水平面内におけるならい
制御方法から、センサ２１の出力はSsとなるよ
うに制御される。したがつて、センサ２１，２２
の出力はともにSsとなつている。

ならい動作が開始され、水平方向のならいが進
作して行くに従つて、センサ２１，２２の出力が
変化する。水平移動距離が第７図に示すように
L₀′となつたところを点R₁′とし、センサ２１の出
力からワーク上の点Q₁′の位置を求める。２つの
センサ２１，２２の出力をそれぞれS_A1、S_B1とす
れば、Q₁′点におけるワークＷの接線の傾きφ₁は、
２つのセンサ２１，２２間の軸間距離Lsを用い
て表わすと、 φ₁＝φ₀＋tan^-1S_B1−S_A1／Ls ……(7) となる。ここで、２つのセンサ２１，２２の出力
差をΔS_H1＝S_B1−S_A1とおき、tan^-1ΔS_H1／Ls＝Δφ₁と おく。

なお、以下の説明においては上記水平方向のな
らい動作距離L₀′を基準姿勢サンプリング距離と
呼ぶことにする。

さて、ロボツト１が点R₁′に到着した時点では
センサ仰角はβ₀である。次に、ならい動作を継続
する。いま、垂直面内のワーク表面曲率変化も、
前回のならい動作区間の曲率変化とほぼ同じであ
るとすれば、点Q₁′からならい動作距離L′だけ移
動した点で、センサ２１，２２の出力差はΔS_H1
になると予想される。垂直面内のならい姿勢制御
とは、このような予想にもとづいて次の姿勢サン
プリング距離L′を選択決定することである。ここ
では、 L′＝Δφc×L₀′／Δφ₁；｜Δφ₁｜≧Δφcのとき L′＝L₀′；｜Δφ₁｜＜Δφcのとき ……(8) となるように、すなわちセンサ出力差から求めた
姿勢サンプリング距離間の角度変化の値に反比例
させて、次の姿勢サンプリング距離L′を定める。

ここに、Δφcは定数であり、１／２tan^-1 Smax−Smin／Lsまたはtan^-1Smax−Smin／2Lsよりも 小さくなるようにその値を定める。

このようにして次の姿勢サンプリング距離L′を
決め、次の姿勢サンプリング点R₂′を定める。
R₂′点は、(8)式により求めた距離L′をL₂′として、
水平方向のならい動作の接線上にとつた道のりが
L₂′になる点として決定する。こうしてロボツト
１が第８図に示すように点R₂′に到達すれば、２
つのセンサデータを再度読み込み、垂直面内での
ワークＷの傾きφ₂及びセンサ仰角β₂（＝φ₂）を計
算し、次の姿勢サンプリング点R₃′を求め、R₃′点
に到着したときにセンサ仰角がβ₂となるようにな
らい動作を行う。このような処理を繰返して行く
ことが、本発明のもう一つの特徴である。

これを一般化すると、第９図のようになる。点
R₁′にロボツト１の手先が到着している時点にお
いて、ワークＷの表面の垂直面内接線の傾きを
φ₁とすれば、この傾φ₁は φ_i＝φ_i-1＋tan^-1ΔS_Hi／Ls＝φ_i-1＋Δφ_i ……(9) である。ΔS_Hi＝S_Bi−S_Aiであり、センサ姿勢の仰
角はβ_i-1＝φ_i-1となつている。また、点Q′_i-1から
点R₁′までの水平方向ならい動作における接線上
にとつた道のりをL_i′とすれば、この道のりが
L₀′である場合の垂直面内ワーク接線の傾きφ_i ^*
は、曲率の変化が均一であると仮定して、近似的
に φ_i ^*＝φ_i-1＋L₀′／L₁′・Δφ_i ……(10) となる。よつて Δφ_i ^*＝φ_i ^*−φ_i-1＝L₀／L_i′・Δφ_i……(11) とおけば、次の姿勢サンプリング区間の道のり
L′_i+1は、(8)式にならつて、 L′_i+1＝Δφc×L₀′／Δφ_i ^* ＝Δφc×L_i′／Δφ_i ；｜Δφ_i ^*｜≧Δφcのとき L′_i+1＝L₀′ ；｜Δφ_i ^*｜＜Δφcのとき ……(12) として求めることができる。これにより、次の姿
勢サンプリング点R′_i+1の位置が得られる。そこ
で点R′_i+1において、センサ仰角β_iがβ_i＝φ_iとなる
ように設定し、ならい動作の姿勢制御を行なう。
(12)式の関係を第１０図に示す。

なお、上記実施例では、ならい動作方向の接線
決定のためのサンプリング距離L_iの決定にあた
り、一般式として(5)式、(6)式を用いたがΔθ_iが微
小角度であるとすれば、近似的に(5)式は Δθ^* _i+1≒L₀／L_i・Δθ_i+1 ……(13) と簡略化してもよく、さらに(6)式においても、 L_i+2≒ΔS₀／Δθ^* _i+1 ；｜Δθ^* _i+1｜≧Δφc L_i+2≒L₀ ；｜Δθ^* _i+1｜＜Δθc ……(14) と、tanΔθ_i+1≒Δθ_i+1の関係を用いて簡略化する
などの方法が考えられる。

これらの式（13）、（14）において、Δθ^* _i+1、
Δφ^* _iなどを計算しないで、Δθ_i+1、Δφ_iの値から直
接的に求める簡略化も考えられる。例えば、前区
間のサンプリング距離をL_iとして、 L_i+1＝L_i−ｋΔθ_i／Δθc 同様に前区間の姿勢サンプリング距離をL′_iとし
て L′_i+1＝L′_i−k′Δθ_i／Δθc を求めることなどは、ｋ、k′を定数とした大まか
な線型近似であるが、これらによつても従来方式
よりはきめの細かい制御が可能であり、一つの簡
略法として有効である。さらに、(6)式において、
｜Δθ^* _i+1｜の値にかかわらずL_i+2＝ΔS₀／tanΔθ^* _i
＋１とし、また(2)式において、｜Δφ^* _i｜の値にかか
わらずL′_i+1＝Δφc・L′₀／Δφ_i ^*とするような方法
であつてもよい。

前記実施例においては、水平方向のならい動作
のならい動作方向決定のための接線演算を行うサ
ンプリング距離と、垂直面内での姿勢決定のため
の姿勢サンプリング距離とをそれぞれL_i、L′_iのよ
うに区別して示したが、これらはL_i、L_i′のうち
の小さい方を両者に共通したサンプリング距離と
して選び、センサ２１，２２のデータを読み出す
回数を削減することも可能である。また、例えば
垂直面内での姿勢を制御することが、作業の仕様
から見てそれほど重要でない場合は、水平方向の
サンプリング距離のみを前述の方法によつて決定
し、これに合わせて垂直面内での姿勢制御のため
の計算を行つたり、あるいは逆の選択方法をとる
などの方法を用いてもよい。

さらに、前述の実施例においては、水平面内に
おけるならい動作と、垂直面内における姿勢制御
の例を示したが、ならい動作は水平面内に限定さ
れるものではなく、任意の平面または曲面内にお
いて行うものであつても、本発明は有効である。
ただし、ならい動作を行う平面または曲面内に、
２本のセンサ２１，２２が同時に含まれないよう
にセンサを配置する必要がある。このために、ロ
ボツト１の手先にセンサ２１，２２をセンサ２１
の主軸まわりに回転させるためのサーボ機構など
を追加する方法をとつてもよい。

上記実施例では、ロボツト１は直角座標形のロ
ボツトを例示しているが、多関節形、極座標形な
どのロボツトであつてもよく、またはレール上を
走行する走行台車などのような作業機械であつて
もよい。

また、センサとして非接触式の近接距離センサ
を用いた例を示したが、センサとしては、ポテン
シヨメータ等をばねにより付勢した触針式セン
サ、超音波式距離センサ、レーザ光や赤外光など
によるスポツト光やスリツト光などをワーク表面
に照射しこれをイメージセンサやテレビカメラな
どでとらえて距離を検出する方式のセンサ、また
は画像処理によりワーク表面の位置を検出する方
式のセンサなど、種々の形式のセンサも用いるこ
とができる。

また、前述の実施例においては、センサ２１，
２２の主軸２１０，２２０がワークＷの表面に対
して常に垂直となる例を示したが、センサの姿勢
はワークの表面に対して一定の角度をなしても良
く、必ずしも完全に垂直な方向から距離検出を行
う必要はない。また、このようにセンサが一定の
姿勢を持つていても、本発明のならい動作制御は
有効である。

以上、本発明の一実施例と、そのいくつかの変
形例について述べたが、本発明によれば、曲率が
複雑に変化する対象物に対して、その曲率に応じ
てならい動作のサンプリング距離を変えながらな
らい動作を行うため、曲率半径の小さいワーク部
分についても、その曲率半径の値に対応した円滑
かつ高精度のならい動作を実現できる。また、接
線の傾きが変化しなくとも、(6)式、(12)式に見られ
るように、サンプリング距離の最大値をそれぞれ
L₀、L₀′としているが、これはならい動作におい
て、一定の長さ以下の折れ線により対象物を近似
するものであり、曲率半径が大きい個所において
も、このようなならい制御により、きめの細かい
高精度のならい動作が可能である。一方、常に微
小距離ごとに接線計算を行う従来の方式では、そ
の接線計算の精度に対するセンサの計測精度の誤
差をまぬがれず誤差が蓄積されるのに対し、本発
明の方法では安定したならい動作制御が可能であ
る。

前述の実施例においては、その機能として、な
らい動作面内における接線検出およびならい動作
面と異なる面内（実施例では垂直面内）における
接線検出の２つが含まれているが、これらのうち
のならい動作平面内における機能のみを実現する
ものであつても、本発明の範囲内にあり、本発明
の簡略化例として十分実用になるものである。

上記各実施例においては、それぞれの演算式か
らなる制御アルゴリズムを第３図計算機５１のソ
フトウエアとして具体化してもよいし、専用の電
子回路を付加して演算処理時間を短縮することも
できる。

前述の各実施例においては、作業工具を図示し
ていないが、例えば塗布用弾性体、塗装ガン、溶
接トーチ、グラインダ、シーリング工具などの作
業工具をロボツトの手先に配置し、これら工具の
作業点位置をロボツト１の手先位置Ｐと一致させ
るか、手先位置Ｐに対して一定の位置関係を持つ
ように配置し、工具の作業点位置をロボツト１の
手先位置とみなして演算制御すると、作業工具に
よる自動ならい作業が可能である。もちろん、作
業工具を特に備えず、センサによる検出データか
らワークＷの形状を計算し、計測用ロボツトとし
て用いる用途に、または、このようにしてワーク
Ｗの形状をワーク上に沿つた点群の位置データと
して求め、産業用ロボツトにおけるテイーチング
作業に代え、このならい動作結果として得られた
動作経路に沿いプレイバツク動作制御によりロボ
ツトに作業を行わせる自動テイーチング方法に本
方法を応用することも可能である。

〔発明の効果〕

本発明によれば、指定面内方向ではセンサの移
動前後の出力から接線方向を検出するので、セン
サがひとつで済み、曲率が急激に変化する部分を
含めて、複雑な３次元形状を有する対象物を正確
にならい制御でき、対象物の最初の微小部分につ
いてならい動作を行なわせれば、そのならい動作
で得られた対象物の形状に関する情報に基づいて
形状を予測しながら自動的に測定するロボツトに
よる形状ならい制御方法が得られる。

【図面の簡単な説明】

第１図は本発明のならい制御装置の一実施例を
備えた作業機械としてのロボツトの一具体例を示
す斜視図、第２図は第１図ならい制御装置および
ロボツトの制御系の構成の一例を示すブロツク
図、第３図は第１図実施例における水平面内のな
らい動作制御方法を示す図、第４図は第３図方法
を一般化して示す図、第５図は本発明のならいサ
ンプリング距離L_i+1の選択にあたつて用いる
tanΔθ_i ^*とL_i+1の関係を示す図、第６図〜第８図
は第１図実施例における垂直面内の姿勢制御方法
を示す図、第９図は第６図〜第８図方法を一般化
して示す図、第１０図は同じく姿勢サンプリング
距離L′_i+1の選択に用いるΔφ_i ^*とL′_i+1との関係を
示す図である。 Ｗ…対象物、Ｐ…ロボツトの手先位置、１…ロ
ボツト、２１，２２，３１，３２，４１，４２…
センサ、５１…制御用計算機、５８…センサ用イ
ンターフエース。

Claims (1)

1. 【特許請求の範囲】 １ 移動体の先端に取付けられ対象物表面との相
   対距離を検出するセンサを指定面内における対象
   物表面の接線に沿つて指定方向に設定移動距離だ
   け移動させ、センサと対象物表面との相対距離お
   よび移動体の３次元空間内の位置に基づいて対象
   物表面の位置を求め、 前記設定移動距離両端に対応する対象物表面の
   位置から当該対象物表面の指定面内における接線
   を演算し直し、当該接線の指定方向の延長上で前
   記設定移動距離だけ離れた点に次の動作目標点を
   設定し、センサを当該動作目標点に向けて移動さ
   せるならい制御方法において、 演算し直した接線とその前に演算された接線と
   の方向の変化量に応じて前記設定移動距離を変更
   することを特徴とするならい制御方法。 ２ 特許請求の範囲第１項に記載のならい制御方
   法において、 前記指定面と交わる面内に前記センサを複数個
   設け、当該センサの出力に基づき３次元形状の対
   象物表面の前記交わる面内における法線方向を演
   算し、前記３次元形状の対象物表面の法線に対し
   て一定の角度以内となるように前記センサの方向
   を規制することを特徴とするならい制御方法。 ３ 特許請求の範囲第１項に記載のならい制御方
   法において、 前記センサの姿勢を前記指定面内における前記
   接線に対して一定の角度となるように規制するこ
   とを特徴とするならい制御方法。 ４ 移動体の先端に取付けられ対象物表面との相
   対距離を検出するセンサと、 前記移動体を制御して前記センサを指定面内に
   おける対象物表面の接線に沿つて動作目標点まで
   移動させる制御手段と、 前記センサと対象物表面との相対距離および移
   動体の３次元空間内の位置に基づき動作目標点に
   対応した対象物表面の位置を求め、相続く２つの
   動作目標点に対応する対象物表面の位置から指定
   面内における対象物表面の接線を求め、当該接線
   の指定方向の延長上で設定移動距離だけ離れた点
   を次の動作目標点に設定して前記制御手段に出力
   する第１演算手段と、 新たに演算する接線とその前に演算された接線
   との方向の変化量に応じて前記設定移動距離を変
   更し前記第１演算手段に出力する第２演算手段と を含むならい制御装置。