JPH0957670A

JPH0957670A - ロボット軌道制御装置及び耐久性試験装置

Info

Publication number: JPH0957670A
Application number: JP21471095A
Authority: JP
Inventors: Yasuyuki Nakada; 康之中田; Mitsuru Shiraishi; 満白石
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 1995-08-23
Filing date: 1995-08-23
Publication date: 1997-03-04

Abstract

(57)【要約】【課題】誤差の少ない円軌道を取得するロボット軌道
制御装置の提供。【解決手段】ロボットの所定箇所の空間位置を検出し
て検出位置とする位置検出手段、位置検出手段により検
出された検出位置を順次記憶する位置記憶手段、位置記
憶手段により記憶された検出位置に基づいて駆動部の探
索方向を算出する探索方向算出手段、駆動部と駆動対象
物との接触力を検出する接触力検出手段、探索方向算出
手段により算出された探索方向について予め設定された
目標力と接触力検出手段により検出された接触力との偏
差をゼロにするロボット駆動のための駆動信号を生成す
る力制御手段を備える。そして、探索方向算出手段は、
粗い周期で往路の軌道探索を行い、往路の軌道探索で得
た軌道を使用して、細かい周期で復路の軌道探索を行
う。復路に探索された回転軌道は誤差が少なく、耐久性
試験に適する回転軌道である。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、回転軸を有する部
材を把持して回転軸を中心に回転させるロボットの軌道
の制御に技術に関する。

【０００２】パソコン、端末装置の軽量化・小型化に伴
い、製品の材料は軽量化・薄肉化される傾向にある。材
料の軽量化・薄肉化によって、特にラップトップパソコ
ン等のように蓋の開閉をする製品では可動部分に力（ス
トレス）が加わるために、強度や耐久性に問題が生じ
る。そこで、設計、試作の段階で、製品の耐久度を予め
調べておくための耐久性試験が必須のものとなる。

【０００３】従来より試作された製品の耐久性試験は、
人手あるいは試験専用機によって行われていた。しか
し、耐久性試験は、ストレスを強制的に加える動作を数
千から数万回の単位で繰り返すため、人手で試験するの
は大きな負担となる。試験専用機によれば、繰り返しに
よる工数的な負担はなくなるが、例えば、上記ラップト
ップパソコンであれば、蓋の回転動作がスムーズに繰り
返されるように試験対象となるパソコンの設定位置を調
整する必要があり、操作が煩雑であった。また、人手や
試験専用機による試験では、蓋の機械的疲労度を測定す
るために、測定の途中でトルク計等の専用の治具を用い
て蓋の回転トルクを測る必要がある。このため、測定時
間がかかりコストがアップする。

【０００４】そこで、従来より試験用のロボットを使用
して耐久性の試験を行うことが行われていた。

【０００５】

【従来の技術】ラップトップパーソナルコンピュータ
（以下「ラップトップパソコン」という。）の蓋ように
回転軸を有する部材を把持し、回転軸を中心として円軌
道を描く動作をクランク回し動作という。耐久性試験の
ために、同一の回転軌道に沿って繰り返しクランク回し
動作をさせるためには、ロボットに円軌道を教示する必
要がある。円軌道を教示するには、以下の２つの方法が
知られている。 i) オペレータがロボットを直接操作して軌道をロボ
ットに教え込む直接教示機能による方法 ii) オペレータの助けを借りること無く、ロボットが
自立的に回転中心座標を獲得し、回転機構により回転動
作を行う方法（特開平６−５１８１９号）上記第１の方法は、ラップトップパソコンの扉のような
ものが対象となる試験部材とロボットとの相対位置及び
把持位置の位置決定を正確に行い、予め教示した円軌道
を駆動させるものである。

【０００６】第２の方法は、クランク回し動作の軌道を
決定するために、ロボットの把持部が通過した点の位置
を算出する座標系算出部を用いる。軌道探索の際には、
ロボットの先端と把持する試験部材との間の接触力から
円軌道の法線方向と接線方向とを算出し、ロボットの先
端が通過した点により構成される軌跡からクランク回し
動作の中心点を推定する。円軌道の中心点の算出方法と
しては、軌道上の３点から中心点を求める方法、円軌道
の直径から中心点を求める方法を用いていた。

【０００７】上記のように、従来のロボット軌道制御方
法では、オペレータの操作による教示又は自動的にクラ
ンク回し動作をさせるための軌跡を装置が求め、円軌道
を決定するというものであった。

【０００８】

【発明が解決しようとする課題】しかしながら、第１の
方法による円軌道の教示方法では、ロボットのクランク
回し動作に必要な部材の指定箇所へのセット、ロボット
への円軌道の教示等、実際にクランク回し動作を実行さ
せるまでに、オペレータが複雑な操作をしなければなら
ないという問題があった。複雑な操作が必要とされる
と、ロボットに教示する円軌道がオペレータ毎にばらつ
き、位置制御が一様に行われないおそれがある。

【０００９】また、上記第２の方法によれば、自立的な
軌道探索の初期における段階のロボットの把持部の移動
する方向が、最終的に求められる円軌道の接線方向と異
なることが多いために、理想的な円軌道から自立的に探
索された軌道が大きくずれて設定されるという問題があ
った。軌道が正確でないと、円軌道の中心点の算出も正
確に行えない。また、自立的に探索動作開始位置からク
ランク回し動作の終点までの軌道（往路）が探索できて
も、復路の制御については具体的にどのように探索を行
うべきかが規定されていなかった。

【００１０】そこで、上記問題点に鑑み本発明は、回転
軸を有する部材を把持し、当該部材の回転軸を中心とし
た円軌道を自動的に取得する際に、理想的な円軌道から
の誤差の少ない円軌道を取得することができるロボット
軌道制御装置及び耐久性試験装置を提供することにあ
る。

【００１１】

【課題を解決するための手段】請求項１に記載の発明
は、回転軸に回転自在に支持された駆動対象物を駆動部
により駆動し、駆動部を移動させることにより駆動対象
物を回転軸を中心に回転させるロボットのためのロボッ
ト軌道制御装置であって、ロボットの所定箇所の空間位
置を検出して検出位置とする位置検出手段と、位置検出
手段により検出された検出位置を順次記憶する位置記憶
手段と、位置記憶手段により記憶された検出位置に基づ
いて駆動部の探索方向を算出する探索方向算出手段と、
駆動部と駆動対象物との接触力を検出する接触力検出手
段と、探索方向算出手段により算出された探索方向につ
いて予め設定された目標力と接触力検出手段により検出
された接触力との偏差をゼロにするロボット駆動のため
の駆動信号を生成する力制御手段と、を備え、探索方向
算出手段は、駆動部が探索開始位置から探索終了位置に
到達するまでの往路の探索動作時には、第１の時間間隔
で探索方向の算出を繰り返し、検出位置の軌跡を往路の
回転軌道として位置記憶手段に記憶させ、探索終了位置
から探索開始位置に到達するまでの復路の探索動作時に
は、第１の時間間隔より短い第２の時間間隔で探索方向
の算出を繰り返し、検出位置の軌跡を復路の回転軌道と
して位置記憶手段に記憶させる。

【００１２】請求項２に記載の発明は、請求項１に記載
のロボット駆動制御装置において、探索方向算出手段
は、位置記憶手段に記憶された往路の回転軌道に基づい
て復路の回転軌道に対応する円の中心点及び円の半径を
求め、円の接線方向を求めて探索方向とする。

【００１３】請求項３に記載の発明は、請求項１に記載
のロボット駆動制御装置において、探索方向算出手段
は、所定の位置から新たに検出された検出位置までの間
に検出された複数の検出位置について、各検出位置との
距離の自乗平均を最小にする位置を回転軌道に対応する
円の中心点とし、中心点より円の半径を求める。

【００１４】請求項４に記載の発明は、請求項１に記載
のロボット駆動制御装置において、前回のサンプリング
時における検出位置と今回のサンプリング時における検
出位置との間の距離が所定値以下になったときに探索動
作を終了する。

【００１５】請求項５に記載の発明は、請求項１に記載
のロボット駆動制御装置において、復路の探索動作時に
おいて、往路の探索開始位置と新たに検出された検出位
置との距離が所定値以下になったときに、探索動作を終
了する。

【００１６】請求項６に記載の発明は、請求項１に記載
のロボット駆動制御装置を備えた耐久性試験装置におい
て、ロボット駆動制御装置は、ロボットに往復動作をさ
せる際に、復路の探索動作により得られた回転軌道の探
索終了位置から探索を開始し、当該回転軌道を経て回転
軌道の探索開始位置まで探索を行う軌道を往路の回転軌
道とし、往路の回転軌道と復路の回転軌道とに従ってロ
ボットに往復動作をさせる。

【００１７】請求項７に記載の発明は、請求項１に記載
のロボット駆動制御装置を備えた耐久性試験装置におい
て、ロボット駆動制御装置は、ロボットに往復動作をさ
せる際に、復路の回転軌道に基づいて等角速度でロボッ
トを駆動するための等角速度回転軌道を生成し、等角速
度回転軌道に基づいてロボットに往復動作をさせる。

【００１８】請求項１に記載の発明によれば、往路の探
索動作を行う場合には比較的長い周期の第１の時間間隔
で探索方向の生成を行う。このため、往路の探索動作で
得られる回転軌道は、ある程度誤差があるものの、初期
の探索方向が求めるべき回転軌道と異なることにより生
ずる精度の低い無駄な探索動作に時間を浪費することが
ない。また、復路の探索動作の開始時において、往路の
探索動作により得られた軌道に基づいて初期の探索方向
が算出されるので、復路の探索動作における初期の探索
方向が本来の探索方向から大きくずれることがない。そ
して、復路の探索動作において、第１の時間間隔よりも
短い周期で探索方法を生成するので、この探索の結果検
出される検出位置から得られる軌道は、そのままロボッ
トの往復動作に用いることができる軌道となる。

【００１９】請求項２に記載の発明によれば、復路の探
索を開始するときには位置記憶手段には往路の回転軌道
が記憶されているので、この軌道を用いれば、回転軌道
に対応する円の中心点及び半径を求められる。中心点及
び半径を用いて算出した円の接線ベクトルは、復路の探
索動作において最初に進むべき方向を示す探索方向であ
る。

【００２０】請求項３に記載の発明によれば、検出位置
の各々から自乗平均を最小にする点は、各検出位置から
等距離にある点、すなわち回転軌道の中心点である。し
たがって、自乗平均を最小にする点を求めれば、円軌道
の中心点及び半径が求められる。

【００２１】請求項４に記載の発明によれば、前回のサ
ンプリング時における検出位置と今回のサンプリング時
における検出位置との距離はすなわちロボットの所定箇
所の移動距離を示す。この距離が所定値以下のときは、
駆動部が移動できない位置（すなわち、終了位置）に位
置すると判断できるので、動作を終了する。

【００２２】請求項５に記載の発明によれば、ロボット
が往復動作を終えた場合、往路の探索動作開始位置と新
たに検出された検出位置との相対距離はほとんどゼロに
なる。したがって、この距離を検出したときには探索動
作は終了したものと判断でき、動作を終了する。

【００２３】請求項６に記載の発明によれば、復路の探
索動作により得られた回転軌道は精度が高いので、これ
を反転した動作を往路の回転動作とすれば、往復動作が
精度の高い軌道に沿って行われる。したがって、このよ
うにして得られた回転軌跡は、駆動対象物の耐久性試験
に適するものとなる。

【００２４】請求項７に記載の発明によれば、復路の回
転軌道は精度が高いので、この軌道に基づいて生成した
等角速度回転軌道も精度が高い。よって、この等角速度
回転軌道は、駆動対象物の耐久性試験に適するものとな
る。

【００２５】

【発明の実施の形態】本発明の好適な実施の形態を図面
を参照して説明する。（Ｉ）構成の説明図１に、本発明の実施の形態におけるロボット駆動制御
のための全体構成図を示す。２００はロボット全体を示
す。ロボット２００は、複数の関節部Ｊ₁〜Ｊ ₃を有す
るマニプレータ２０４の先端部にハンド２０２を備えて
構成される。

【００２６】ハンド２０２は、ロボット軌道制御装置１
００より供給される駆動信号に基づいて駆動対象物２０
１を把持する。駆動対象物２０１は、例えば、ラップト
ップパソコンの扉のように、耐久性試験の対象物であっ
て、所定の回転軸を中心として回動自在に設けられるも
のである。マニプレータ２０４には、複数の関節部から
なり、各関節部には各々モータが設けられている。各モ
ータを駆動することにより、ロボットを駆動する。ハン
ド２０２の先端部には、位置を検出するための位置検出
部２０６（図２、図３参照）を備える。力覚センサ２０
３は、駆動対象物２０１の回転軌道の接線方向である探
索方向についてのハンド２０２の押す力を検出して力検
出信号としてロボット軌道制御装置１００に転送する。

【００２７】ロボット軌跡駆動制御装置１００の構成を
具体的に説明する。本形態では、駆動対象物２０１の探
索開始位置から探索終了位置までの駆動を行う往路の探
索動作に加わる構成要素と、駆動対象物２０１の探索終
了位置から再び探索開始位置へ戻るまでの復路の探索動
作に加わる構成要素が異なる。

【００２８】図２に、ロボット軌道制御装置１００にお
ける往路の探索動作時のブロック図を示す。ロボット２
００において、位置検出部２０６は、その内部構成を図
示しないが、制御対象たるロボットの関節部に備えられ
たモータの回転角を検出するエンコーダ、エンコーダの
出力をカウントするカウンタ及び関節部の回転角からロ
ボットの基準座標系へ座標値を変換する変換部により構
成される。基準座標系に変換された座標値は、位置検出
信号Ｓ_dとしてロボット軌道制御装置１００に転送され
るので、ロボット軌道制御装置１００は基準座標系にお
いてロボットの先端位置を認識する。

【００２９】なお、作業（力覚センサ）座標系はＸ、
Ｙ、Ｚの三座標で構成される。図１に示すように、ロボ
ットの先端部の回転軌跡を含む平面について、回転の中
心軸方向をＸ方向としその変位量をＸ座標で示す。回転
軌道の探索方向における接線方向をＹ方向としその変位
量をＹ座標で示す。また、同図の垂直方向をＺ方向とし
てその変位量をＺ座標で示す。

【００３０】ロボット軌道制御装置１００において、位
置記憶部１は、ロボットの先端位置を示す位置検出信号
Ｓ_dを適当なサンプリング時間間隔で記憶する。サンプ
リング時間としては、当該ロボット軌道制御装置１００
のシステムクロックに同期したサンプリング間隔でも、
これに非同期の時間間隔であってもよい。

【００３１】往路軌跡記憶部２はディスク装置等の大容
量の記憶装置により構成され、位置記憶部１に記憶され
ているロボットの先端位置を回転動作の軌跡として順次
記憶する。

【００３２】探索方向生成部３は、往路軌跡記憶部２に
記憶された軌跡及び位置検出部２０６から転送された基
準座標系でのロボットの先端位置に基づいて、探索方向
を生成する。

【００３３】力制御部４は、探索方向生成部で生成され
た探索方向へ予め設定された目標力を発生されるための
駆動信号Ｄを発生する。操作部２０５は、図示しない補
償器、Ｄ／Ａコンバータ、増幅器、サーボモータ等によ
り構成され、力制御部４から転送された駆動信号Ｄに基
づく速度指令に追従するように、ロボット２００の各関
節部に備えられたモータを駆動する。

【００３４】図３に、ロボット軌道制御装置１００にお
ける復路の探索動作時のブロック図を示す。図３におい
て、図２に示した構成と同じ符号を付した構成要素は、
同様の動作を行うので、説明は省略する。

【００３５】円パラメータ算出部６は、往路に探索して
得た軌跡データを往路軌跡記憶部２から読出して、これ
に基づいて円軌道の中心点及び円の半径を求める。往路
軌跡記憶部５は、ロボット２００が駆動される過程で位
置検出部２０６により検出されるロボットの先端位置を
往路の軌跡の獲得時より短い周期で検出し、さらに滑ら
かな動作をするための復路の軌跡を記憶する。（ＩＩ）往路探索動作次に、本形態の具体的な動作を説明する。

【００３６】本発明では、往路の軌道探索時には、探索
方向の切換を数秒（Ｔ_CH）間隔の粗い単位で行い、粗い
サンプリング間隔で回転軌跡を得る。この往路の回転軌
跡を利用して、復路の軌道探索時には、探索方向の切換
を数百ｍｓ間隔の短い周期で行って、最終的に滑らかな
回転軌跡を得る。

【００３７】往路の軌道探索動作を図４のフローチャー
トに基づいて説明する。往路の軌道探索時には、ロボッ
ト軌道制御装置１００は図２に示すブロック図に従って
動作する。

【００３８】まず、駆動対象物２０１の耐久性試験を実
施するにあたり、オペレータはロボット２００に対し動
作条件を図示しない入力装置を用いて設定する（ステッ
プＳ１）。動作条件としては、探索開始を始めるにあた
り最初にロボットの先端部を駆動する方向を定める探索
開始方向と駆動対象物２０１を駆動していく間に駆動対
象物を探索方向に押す力を目標値として設定する。初期
値たる探索開始方向と大きさが得られれば、探索方向生
成部３は目標力ベクトルを算出し、操作部２０５は目標
力ベクトルに対応させてロボット２００を駆動する（ス
テップＳ２）。

【００３９】ロボットの先端位置の移動に伴って位置検
出部２０６からは位置検出信号Ｓ_dが入力される。位置
記憶部１は現在位置Ｐ_STを獲得し（ステップＳ３）、往
路軌跡記憶部２は往路の軌跡の保存を開始する（ステッ
プＳ４）。

【００４０】いま、基準座標系で表現されたロボットの
先端位置をＰ_ar＝［Ｐ_arｘ、Ｐ_arｙ、Ｐ_arｚ、Ｐ_arα、Ｐ_arβ、Ｐ_arγ］で表し、これを記憶するものとする。但し、添え字の
ｘ、ｙ、ｚは基準座標系のＸ座標、Ｙ座標及びＺ座標で
あり、α、β、γはＸ軸周りの回転角、Ｙ軸周りの回転
角及びＺ軸周りの回転角を示すものとする。

【００４１】ロボットの初期位置、探索開始からｔ秒後
のロボットの先端位置を各々Ｐ_ar（０）＝［Ｐ_arｘ（０）、Ｐ_arｙ（０）、Ｐ_arｚ（０）、Ｐ_arα（０）、Ｐ_arβ（０）、Ｐ_arγ（０）］Ｐ_ar（ｔ）＝［Ｐ_arｘ（ｔ）、Ｐ_arｙ（ｔ）、Ｐ_arｚ（ｔ）、Ｐ_arα（ｔ）、Ｐ_arβ（ｔ）、Ｐ_arγ（ｔ）］で示すこととする。往路軌跡記憶部２は、先端位置を絶
対軌跡と相対軌跡との双方に変換して記憶する。絶対軌
跡は、ロボットの先端位置について基準座標系上での絶
対位置を示す。相対軌跡は、ロボットの先端位置の回転
運動の開始位置からの相対移動量を示す。往路軌跡記憶
部２に記憶される絶対軌跡はＰ_ar（ｔ）の時系列データ
で与えられ、相対軌跡はＰ_ar（ｔ）−Ｐ_ar（０）の時系
列データで与えられる。サンプリング時間毎に記憶され
る位置情報は離散値である。絶対軌跡を取り込むときの
第ｉ番目のサンプリング時のデータは、Ｐ_i＝［Ｘ_i、Ｙ_i、Ｚ_i、α_i、β_i、γ_i］で与えられ、相対軌跡を取り込むときの第i サンプリン
グ時のデータは、Ｐ_i＝［Ｘ_i−Ｘ₀、Ｙ_i−Ｙ₀、Ｚ_i−Ｚ₀、α_i−
α₀、β_i−β₀、γ_i−γ₀］で与えるものとする。但し、Ｐ₀＝［Ｘ₀、Ｙ₀、
Ｚ₀、α₀、β₀、γ₀］は、探索動作開始時の初期位
置である。

【００４２】ステップＳ５において、現在までに得られ
ている探索方向に向けての力制御動作が行われる。ハン
ド２０２の力制御動作が終了すると、一定時間Ｔ_CHが経
過しか否かが検査され（ステップＳ６）、経過した場合
（ＹＥＳ）には軌跡の保存を一時終了する（ステップＳ
７）。この一定時間Ｔ_CHは往路の軌道探索動作時におけ
る軌道探索の周期を規定する。往路の軌道探索動作にお
いては、この一定時間Ｔ_CHを数秒とするので、往路の軌
道探索は本来求めるべき回転軌道に比べると、ある程度
誤差を含んだものとなる。さらに、前回までに得られた
回転軌跡と今回取得した軌跡とを結合し（ステップＳ
８）、新たな探索方向の生成が行われる（ステップＳ
９）。

【００４３】探索方向を求めるために、回転軌道（円軌
道）の接線ベクトルを求める必要がある。回転軌跡の接
線ベクトルは、往路軌跡記憶部２に記憶された軌跡デー
タに基づいて、探索方向生成部３により以下の過程を経
て算出される。

【００４４】往路軌跡記憶部の軌跡が、点の列Ｐ_j＝
（Ｘ_j、Ｙ_j、Ｚ_j）（ｊ＝１，２，…，ｎ（ｎはサン
プリング総数））で表される場合、各サンプリング点と
の距離の和の２乗が最小となるような平面Ｓを求める。
平面Ｓの式を次式のようにおく。

【００４５】ｃ₁Ｘ＋ｃ₂Ｙ＋ｃ₃Ｚ＋ｃ₄＝０、ｃ₃＝１（1.1 ）平面ＳはＸ軸とＹ軸とを含む平面に平行でない面である
とする。定数ｃ₁、ｃ₂、ｃ₄は線形多項式Ｚ＝−ｃ₄−ｃ₁Ｘ−ｃ₂Ｙ（1.2 ）に対し、多重線形回帰分析を適用することによって得ら
れる。

【００４６】点列Ｐ_jの各座標値の関係を次式で表す。
但し、Ｘ_j＝ｘ₁（ｊ）、Ｙ_j＝ｘ ₂（ｊ）、Ｚ_j＝ｙ
（ｊ）とし、各サンプリング点の重み係数をｗ_j、重み
係数の総和をＳ_w、／ｙと／ｘ_iを重みを考慮したｙ、
ｘ_iの平均値とする。

【００４７】

【数１】重み係数を重み係数の平均で割った値をｗｗ_jとする
と、ｗｗ_j＝ｗ_j／（Ｓ_w／ｎ）で表され、自由度ｆをｆ＝ｎ−１で表して、ｙ（ｊ）及びｘ（ｊ）の標準偏差σ_yを求め
ると次式のように表せる。

【００４８】

【数２】式（1.12）のＲ_iは線形相関係数である。式（1.11）を
用いて、 σ_xx（ｐ，ｑ）＝ｆ・（σ_xp・σ_xq）（1.13）とおく。式（1.9 ）（1.10）のｘｘ_i（ｊ）、ｗｘ
_i（ｊ）を要素に持つ行列の積をＷＸＸとすると、ＷＸＸ＝ＷＸ・ＸＸ^T となる。但し、ＷＸ（ｐ，ｑ）＝ｗｘ_p（ｑ）、ＸＸ
（ｐ，ｑ）＝ｘｘ_p（ｑ）である。ＷＸＸの要素を、ｗ
_xx（ｐ、ｑ）とおいて、行列Ａを得る。

【００４９】Ａ（ｐ，ｑ）＝ｗ_xx（ｐ，ｑ）／σ_xx（ｐ，ｑ） (1.14 ）行列Ａの逆行列Ｂの要素をＢ（ｐ，ｑ）とすると、係数
ａ_pは次式で与えられる。

【００５０】

【数３】式（1.2 ）の係数ｃ₁、ｃ₂、ｃ₄は、ｃ₁＝−ａ₁、ｃ₂＝−ａ₂、ｃ₄＝−ａ₀ （1.18）として算出される。

【００５１】次に、平面Ｓにサンプリング点から垂線を
下ろしたときに得られる垂線の射影点の座標値を求める
（図６参照）。サンプリング点をＰ_i＝（Ｘ_i、Ｙ_i、Ｚ_i）とし、平面Ｓをｃ₁ｘ＋ｃ₂ｙ＋ｃ₃ｚ＋ｃ₄＝０とおくと、垂線Ｌの式は次式のように与えられる。

【００５２】（ｘ−Ｘ_i）／ｃ_i＝（ｙ−Ｙ_i）／ｃ₂＝（ｚ−Ｚ_i）／ｃ₃ （1.19）射影点ｐ_i（ｘ_i、ｙ_i、ｚ_i）は、ｘ_i＝Ｘ_i−ｃ₁・σ （1.20）ｙ_i＝Ｙ_i−ｃ₂・σ ｚ_i＝Ｚ_i−ｃ₃・σ σ＝（ｃ₁Ｘ_i＋ｃ₂Ｙ_i＋ｃ₃Ｚ＋ｃ₄）／（ｃ₁ ²ｃ₂ ²＋ｃ₃ ²）となる。平面Ｓ上で適当なｘｙ座標を定めて、射影点ｐ
_iのｘｙ座標系での座標値を求める。平面Ｓ上のｘｙ座
標の各軸の単位ベクトルｘ_s、ｙ_sを次のようにして求
める。平面Ｓの法線ベクトルｎは平面Ｓの式の係数
ｃ₁、ｃ₂、ｃ₃を用いて、ｎ＝（ｃ₁、ｃ₂、ｃ₃）／（ｃ₁ ²＋ｃ₂ ²＋ｃ₃ ²）^1/2 （1.21）で表される。各単位ベクトルはｎに垂直であることを利
用して、ｘ_s＝（−ｃ₂、ｃ₁、０）／（ｃ₁ ²＋ｃ₂ ²）^1/2（1.22）ｙ_s＝ｎ×ｘ_s／｜ｃ×ｘ_s｜（1.23）と定める。射影点ｐi の平面Ｓ上のｘｙ座標上での座標
値（ｘ_si、ｙ_si）は位置ベクトルとｐ_iと、単位ベクト
ルｘ_s、ｙ_sの内積を求めることで得られる。

【００５３】ｘ_si＝ｘ_s・ｐ_i （1.24）ｙ_si＝ｙ_s・ｐ_i （1.25）点列（ｘ_si、ｙ_si）から円の中心（ｃ_x、ｃ_y）及び半
径Ｒを最小２乗法を用いて算出する。円の中心（ｃ_x、
ｃ_y）が定まれば、半径Ｒは次式に基づいて求めること
ができる。

【００５４】

【数４】このとき、半径Ｒの二乗と、各点列（ｘ_si、ｙ_si）と円
の中心（ｃ_x，ｃ_y）との間の距離の二乗との偏差Ｅ_i
は、Ｅ_i＝Ｒ²−｛（ｘ_si−ｃ_x）²＋（ｙ_si−ｃ_y）²｝（1.27）である。偏差の二乗の総和は、

【００５５】

【数５】ここで、ｕ_i＝ｘ_si−ｗ_x、ｖ_i＝ｙ_si−ｗ_x、ｔ_i＝（Ｗ_x−
ｘ_si ²）＋（Ｗ_y−ｙ _si ²）ａ₁＝２Σｕ_i ²、ｂ₁＝２Σｕ_iｖ_i、ｂ₂＝２Σｖ
_i ²、ｄ₁＝Σｕ_iｔ_i、ｄ₂＝Σｖ_iｔ_i とおくと、式（1.29）（1.30）は式（1.31）のような連
立方程式となり、これを解くと円の中心（ｃ_x、ｃ_y）
は式（1.32）、半径Ｒは式（1.33）のように求められ
る。

【００５６】

【数６】円の中心の位置ベクトルＰ_Cをロボットの基準座標系で
表すと、Ｐ_C＝ｃ_xｘ_s＋ｃ_yｙ_s ＝（ｘ_C、ｙ_C、ｚ_C）（1.34）である。ロボットの先端の現在位置をＰ_curとおくと、
円の中心と現在位置とを結ぶ単位ベクトルは（Ｐ_cur−Ｐ_C）／｜Ｐ_cur−Ｐ_C｜（1.35）で与えられるので、円軌道の接線ベクトルｔ_curは、円
の中心と現在位置とを結ぶベクトルと平面Ｓの法線ベク
トルｎとの外積により表される。

【００５７】ｔ_cur＝ｎ×［（Ｐ_cur−Ｐ_C）／｜Ｐ_cur−Ｐ_C｜］（1.36）ステップＳ１０において、探索方向生成部３は、式（1.
36）に基づいて得られた円軌道の接線ベクトルｔ_curが
異常な値でないかをチェックした後、目標ベクトルを算
出する（ステップＳ１１）。すなわち、力制御部４は力
覚センサ２０３の検知した力検出信号をロボットの基準
座標系で記述された力に変換する。例えば、６軸力覚セ
ンサの場合は、各軸についての電気信号Ｅｆは、アンプ
及びＡ／Ｄ変換器によってデジタル信号に変換される。
変換されたデジタル信号を力覚センサについての座標系
として表現するために、力覚センサの各軸の間の干渉を
構成する構成行列Ｋ_sを作用させる。さらに、算出した
力を基準座標Ｋで表すために、構成後の力に座標変換行
列を作用させる。座標変換行列には基準座標系上で記述
された力覚センサの座標系の各座標軸についての単位ベ
クトルが用いられる。力覚センサの座標系のＸ軸、Ｙ
軸、Ｚ軸についての単位ベクトルをそれぞれｎ、ｏ、ａ
とおくと、力覚センサ座標系表現におけるベクトルを基
準座標系の表現に変換する座標変換行列^oＲ_sは

【００５８】

【数７】で表される。以上から基準座標系の力ベクトルＦ_arは、
Ａ／Ｄ変換器のゲインをＧ_Aとすると、Ｆ_ar＝^oＲ_s・Ｋ_s・Ｇ_A・Ｅ_f （2.2 ）で与えられる。さらに、力制御部４は目標値と力覚セン
サに作用する力の偏差がゼロになるような指令値を算出
する。オペーレタがステップＳ１にて与えた目標値の大
きさをＦ_opとすると、目標力ベクトルＦ_dは、式（1.3
6）で求めた探索方向についての接線ベクトルｔ_curを
用いて、Ｆ_d＝Ｆ_op・ｔ_cur （2.4 ）となる。探索方向生成部３はこの目標力ベクトルＦ_dを
力制御部４に供給する。力制御部４は、目標力ベクトル
Ｆ_dと力覚センサ２０３からの力検出信号Ｓ_pに基づい
て駆動信号Ｄを生成する。このとき、検出された力ベク
トルＦ_arとＦ_dとの偏差ベクトルｄ_Fは、ｄ_F＝Ｆ_d−Ｆ_ar である。力制御のゲインをＧ_f（ｓ）とおくと、操作部
２０５に出力すべき速度指令ベクトルｖ_fは、ｖ_f＝Ｇ_f（ｓ）・ｄ_F （2.5 ）となる。操作部２０５は、駆動信号Ｄの示す速度指令ベ
クトルｖ_fに対応させてハンド２０２を駆動する。

【００５９】ステップＳ１２において、新たなロボット
先端の現在位置Ｐ_ENが取得され、探索開始位置Ｐ_STと現
在位置Ｐ_ENとの距離が一定距離以上離れているか否かが
検査される（ステップＳ１３）。この一定距離とは、耐
久性試験において、駆動対象物２０１を一端から他端に
まで駆動する間に移動する距離をいう。この一定距離以
上現在位置Ｐ_ENが前回の探索開始位置Ｐ_STから離れたと
いうことは予定通りの距離を移動したことを示す。した
がって、一定距離以上離れていない場合は（ステップＳ
１３：ＹＥＳ）ロボットが移動不可能な位置に達したこ
とを示すので処理を終了する。また、一定距離以上離れ
ていない場合（ステップＳ１３：ＹＥＳ）は探索動作の
途中であると判断できるので、次の現在位置Ｐ_STを取得
する（ステップＳ３）。（ＩＩＩ）復路探索動作復路の軌道探索動作を図５のフローチャートに基づいて
説明する。復路の軌道探索動作時には、当該ロボット軌
道制御装置１００は、図３に示すブロック図にしたがっ
て動作する。

【００６０】本発明における復路の軌道探索は、比較的
長い（数秒毎）サンプリング周期毎に計算された往路の
軌跡を用いる。まず最初に、円パラメータ算出部６は、
往路軌道記憶部２から往路の軌道データを読出し（ステ
ップＳ２１）、円パラメータの算出を行う（ステップＳ
２２）。円パラメータとは、往路の軌跡に基づいて得ら
れる円軌道の中心点及びその半径を求める計算のことを
いう。円パラメータ算出部６は探索方向生成部３が行っ
た上記した往路の軌道探索動作において説明した処理と
ほぼ同様の処理を行って、探索すべき円軌道の中心位置
及び円の半径を求める。探索方向生成部３は、円パラメ
ータ算出部６の算出した円軌道のパラメータに基づいて
最初にロボットを駆動する方向を指令する目標力ベクト
ルを算出する（ステップＳ２３）。力制御部４は、予め
設定された目標力で、復路の初期の探索方向にロボット
の先端部を駆動するための駆動信号Ｄを出力する。

【００６１】操作部２０５が駆動信号Ｄの示す速度指令
ベクトルに対応させロボット２００の駆動を開始する
と、移動を開始したハンド２０２の位置を検出した位置
検出部２０６から探索方向生成部３へ位置検出信号Ｓ_d
が出力される。位置記憶部１は、検出位置を記憶し、復
路軌跡記憶部５は位置記憶部１に格納された現在位置を
順次記憶していく（ステップＳ２４）。

【００６２】ロボットの先端位置が変化していく探索動
作に入ると、探索方向生成部３は、位置検出部２０６の
検出した現在位置Ｐ_STを取得し（ステップＳ２５）、探
索方向の算出を行う（ステップＳ２６）。この算出値が
異常な値であるか否かをチェックする（ステップＳ２
７）。

【００６３】ステップＳ２８では、探索方向を示す接線
ベクトルに従って目標力ベクトルが算出され力制御部４
へ供給される。ステップＳ２９では、力制御部４が目標
力ベクトルを参照して速度指令ベクトルを生成し、操作
部２０５が速度指令ベクトルにしたがってロボットを駆
動する。

【００６４】一回の力制御が終了すると一定時間Ｔ_CHが
経過するまで待つ（ステップＳ３０：ＮＯ）。一定時間
Ｔ_CHが経過すると（ステップＳ３０：ＹＥＳ）、まず現
在位置Ｐ_ENの取得が先に行われ（ステップＳ３１）、前
回のサンプリング時における位置Ｐ_STと新たに取得した
現在位置Ｐ_ENとが一定距離以上離れている場合（ステッ
プＳ３２：ＹＥＳ）には再度方向探索処理が行われる
（ステップＳ２５〜Ｓ２９）。一定距離以上離れていな
い場合（ステップＳ３２：ＮＯ）、現在位置Ｐ_ENと往路
の動作開始位置との距離を検査する。一定距離以上離れ
ている場合（ステップＳ３３：ＹＥＳ）、まだ往路の動
作開始位置にまでロボットの先端が達していないことを
示しているので、探索処理を続行する（ステップＳ２９
〜）。一定距離以上離れていない場合（ステップＳ３
３：ＮＯ）、往路の動作開始位置に到達したことを示し
ているので、軌跡の保存を終了し（ステップＳ３４）、
復路の探索動作を終了する。（ＩＶ）再生動作往復の軌道探索動作によって得られた軌跡に基づいて行
う往復動作を「再生動作」という。再生動作の軌跡は、
細かいサンプリング周期で行われた復路の探索による軌
跡データを用いる。復路の再生動作の軌跡は復路の探索
動作による軌跡に基づいて求める。往路の再生動作の軌
跡は、復路の探索動作による軌跡データを反転したデー
タに基づいて求める。

【００６５】再生動作用の軌跡を求める方法として、往
路探索動作における回転軌道の軌跡と復路探索動作にお
ける回転軌道の軌跡とを合成した軌跡そのものを用いる
方法と、角速度回転軌跡を用いる方法とがある。本形態
にはいずれの方法を用いてもよい。

【００６６】軌跡の再生動作を実施するときは、復路軌
跡記憶部５に記憶した復路探索動作による回転軌跡の点
列を、システム内部のＲＡＭ等のメモリ（図示せず）に
一旦記憶する。位置サーボ系が動作している最中に、シ
ステムが軌跡データを蓄積した記憶部を直接アクセスし
た場合、記憶部に記憶された軌跡データを読出して転送
するまでに時間を要し、１サイクルの処理が１サンプリ
ング時間内に終了しない現象が生ずる。軌跡データを一
旦メモリにセットするのは、これを防止するためであ
る。

【００６７】軌跡データを記憶部から読出しメモリにセ
ットする場合、以下に掲げる項目についてチェックす
る。軌跡の種類についてのチェック往路軌跡記憶部２から読出される軌跡データが絶対位置
表現であるか相対位置表現であるかによって、メモリに
セットする値を変更する。軌跡データが絶対位置表現で
ある場合、記憶部２からのデータをそのままの値でメモ
リにセットする。また、軌跡データが相対位置表現であ
る場合には、ロボットの現在位置を読み込み、読み込ん
だ位置検出信号Ｓ_dの示す位置と記憶データの示す位置
の和をメモリにセットする。ロボットの現在位置と軌跡パターンの先頭データとの
比較ロボットの現在位置と軌跡の先頭データが示す位置とが
一致しているか否かをチェックする。動作開始位置にロ
ボットの先端部（ハンド）が位置しない場合は、動作を
解するための基本位置にロボットをセットする必要があ
るからである。軌跡データに基づく動作範囲のチェック記憶部から得られた軌跡データにより示される動作軌跡
が、ロボットの可動範囲内にあるか否かをチェックす
る。可動範囲になければ、軌跡データが異なるかロボッ
トの調整がずれていることを意味するからである。

【００６８】ロボット軌道制御装置は、上記のチェック
を経てメモリに格納された動作点列の中ら各サンプリン
グ時における次の目標位置を読み込んで速度指令を生成
し、位置サーボ系へ制御信号を供給してロボットを駆動
する。再生動作における速度指令ベクトルの生成につい
て説明する。

【００６９】メモリに格納された軌跡を示す点列のう
ち、先頭の点から第ｉ番目に存在する軌跡の位置ベクト
ルをｐ_Ii、軌跡を取得するサンプリング時間をｔ_sとお
くと、軌跡の再生動作中の第ｉサイクルにおいて得られ
る速度指令ベクトルｖ_oa（ｉ）は、ｖ_oa（ｉ）＝（ｐ_Ii+1−ｐ_Ii）／ｔ_s （3.1 ）で求められる。なお、サーボ系のサンプリング時間と軌
跡を記憶部から取得するサンプリング時間とが相違する
ときは、式（3.1 ）の計算を時間ｔ_s間隔で行う。軌跡
の再生動作において、目標とする軌跡が基準座標系で記
述されているので、ロボットの作業軌跡を基準座標系に
一致させる。したがって、作業座標系の各座標軸の単位
ベクトルｎ_w、ｏ_w、ａ_wは、式（3.2 ）のようにな
る。

【００７０】

【数８】位置サーボ系では、式（3.1 ）のようにして算出された
速度指令と、現在のサンプリング時の目標到達位置及び
現在のロボットの位置の間の位置ズレをゼロにするため
の速度指令との和を計算して、ロボットを駆動する。再
生動作開始時のロボットの位置ベクトルをＰ_cr、ロボッ
トの現在位置がＰ_arであるとき、位置Ｐ _crを基準とした
相対目標移動量ｖ_oint（ｔ）は、ｖ_oint（ｔ）＝∫ｖ_oaｄｔ（3.3 ）で与えられるので、位置の偏差ベクトルｄ_pは、ｄ_p＝（Ｐ_cr＋ｖ_oint）−Ｐ_ar （3.4 ）で求められる。また、位置の積分偏差ベクトルｄ
_intは、ｄ_int＝∫ｄ_pｄｔ（3.5 ）である。従って、位置の比例ゲインをｇ_pp、積分ゲイン
をｇ_piとおくと、操作部２０５の速度指令ｖ_kは、ｖ_p＝ｇ_pp・ｄ_p＋ｇ_pi・ｄ_int （3.6 ）となる。（Ｖ）その他の形態本発明の上記形態に限らず種々の変形が可能である。

【００７１】例えば、往路の探索動作を粗く行い復路の
探索動作を細かく行う本発明は、クランク回し動作に限
るものではない。すなわち、ロボット等がほぼ同一の経
路を往復する動作を行う場合であれば、本発明の技術的
思想を適用することが可能である。往路の軌跡を記録
し、これに基づいて復路の軌道を得て、復路の探索動作
時に細かく軌跡の検出を行うことで、本発明と同等の効
果を得ることができる。

【００７２】

【発明の効果】請求項１及び請求項２に記載の発明によ
れば、ロボットが一定条件下で自動的に回転軌跡を取得
するため、人手による直接教示に比べ、軌跡のばらつき
が少ない。また、ロボットが自動的に回転軌跡を取得す
るため、回転動作を伴う作業をロボットに教示するとき
のオペレータの負担が軽減される。特に、往路に得られ
た回転軌道を参照して復路の探索方向を算出するので、
復路の初期の探索方向をほぼ正しい方向から始められ、
復路の探索軌道に誤差が生じにくい。

【００７３】請求項３に記載の発明によれば、最小自乗
平均を求めるので、従来より正しく円の中心点を把握で
きる。請求項４及び請求項５に記載の発明によれば、探
索位置の変化量を判定するので、自動的に探索の終了を
判断できる。

【００７４】請求項６及び請求項７に記載の発明によれ
ば、誤差のない回転軌道が得られるので、同一の動作を
多数回繰り返す耐久性試験に適する試験装置を提供でき
る。

【図面の簡単な説明】

【図１】ロボット軌道制御の全体図である。

【図２】往路探索動作時のブロック図である。

【図３】復路探索動作時のブロック図である。

【図４】往路探索時の処理を示すフローチャートであ
る。

【図５】復路探索時の処理を示すフローチャートであ
る。

【図６】サンプリング点と射影点との関係を示す図であ
る。

【符号の説明】

１…位置記憶部２…復路軌跡記憶部３…探索方向生成部４…力制御部５…復路軌跡記憶部６…円パラメータ算出部１００…ロボット軌道制御装置２００…ロボット２０１…駆動対象物２０２…ハンド２０３…力覚センサ２０４…マニプレータ

Claims

【特許請求の範囲】

【請求項１】回転軸に回転自在に支持された駆動対象
物を駆動部により駆動し、前記駆動部を移動させること
により前記駆動対象物を前記回転軸を中心に回転させる
ロボットのためのロボット軌道制御装置であって、前記ロボットの所定箇所の空間位置を検出して検出位置
とする位置検出手段と、前記位置検出手段により検出された前記検出位置を順次
記憶する位置記憶手段と、前記位置記憶手段により記憶された前記検出位置に基づ
いて前記駆動部の探索方向を算出する探索方向算出手段
と、前記駆動部と前記駆動対象物との接触力を検出する接触
力検出手段と、前記探索方向算出手段により算出された探索方向につい
て予め設定された目標力と前記接触力検出手段により検
出された接触力との偏差をゼロにするロボット駆動のた
めの駆動信号を生成する力制御手段と、を備え、前記探索方向算出手段は、前記駆動部が探索開始位置か
ら探索終了位置に到達するまでの往路の探索動作時に
は、第１の時間間隔で前記探索方向の算出を繰り返し、
前記検出位置の軌跡を往路の回転軌道として前記位置記
憶手段に記憶させ、前記探索終了位置から前記探索開始
位置に到達するまでの復路の探索動作時には、前記第１
の時間間隔より短い第２の時間間隔で前記探索方向の算
出を繰り返し、前記検出位置の軌跡を復路の回転軌道と
して前記位置記憶手段に記憶させることを特徴とするロ
ボット駆動制御装置。
【請求項２】請求項１に記載のロボット駆動制御装置
において、前記探索方向算出手段は、前記位置記憶手段に記憶され
た前記往路の回転軌道に基づいて復路の回転軌道に対応
する円の中心点及び当該円の半径を求め、当該円の接線
方向を求めて前記探索方向とすることを特徴とするロボ
ット駆動制御装置。
【請求項３】請求項１に記載のロボット駆動制御装置
において、前記探索方向算出手段は、所定の位置から新たに検出さ
れた検出位置までの間に検出された複数の検出位置につ
いて、各前記検出位置との距離の自乗平均を最小にする
位置を前記回転軌道に対応する円の中心点とし、当該中
心点より当該円の半径を求めることを特徴とするロボッ
ト駆動制御装置。
【請求項４】請求項１に記載のロボット駆動制御装置
において、前回のサンプリング時における検出位置と今回のサンプ
リング時における検出位置との間の距離が所定値以下に
なったときに探索動作を終了することを特徴とするロボ
ット駆動制御装置。
【請求項５】請求項１に記載のロボット駆動制御装置
において、前記復路の探索動作時において、往路の探索開始位置と
新たに検出された検出位置との距離が所定値以下になっ
たときに、探索動作を終了することを特徴とするロボッ
ト駆動制御装置。
【請求項６】請求項１に記載のロボット駆動制御装置
を備えた耐久性試験装置において、前記ロボット駆動制御装置は、前記ロボットに往復動作
をさせる際に、前記復路の探索動作により得られた回転
軌道の前記探索終了位置から探索を開始し、当該回転軌
道を経て当該回転軌道の前記探索開始位置まで探索を行
う軌道を前記往路の回転軌道とし、当該往路の回転軌道
と前記復路の回転軌道とに従って前記ロボットに往復動
作をさせることを特徴とする耐久性試験装置。
【請求項７】請求項１に記載のロボット駆動制御装置
を備えた耐久性試験装置において、前記ロボット駆動制御装置は、前記ロボットに往復動作
をさせる際に、前記復路の回転軌道に基づいて等角速度
で前記ロボットを駆動するための等角速度回転軌道を生
成し、当該等角速度回転軌道に基づいて前記ロボットに
往復動作をさせることを特徴とする耐久性試験装置。