JPS63157208A - 多関節ア−ムの振動防止装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は多関節マニピュレータの振動防止方法に係り、
特に、各関節の剛性が低く、冗長自由度を持つ長尺のマ
ニピュレータに好適な振動防止装置に関する。

【従来の技術〕

従来、考えられている装置では、特開昭５９−’　２０
６９０２号公報に記載のように、動力伝達機構のモータ
側の位置、又は、角度及び負荷側の位置、又は、角度を
ともに入力側にフィードバックする方式となっていた。

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

上記従来技術は、振動が発生したらいかに早期にその振
動を減衰させるかに主眼が置かれており。

負荷側に設置した角度、又は、加速度センサにより検出
した信号と目標値信号との差をモータの操作量として出
力する方式をとっている。この方式では、振動が発生す
る前に加振力を予測できないため、一旦、振動が発生し
たことを検出して振動防止制御をしており、振動の発生
は充分には防止できないという問題があった。

本発明の目的は予め加振力を予測し、予測した加振力に
基づいて操作力を制御することにより、はぼ完全に多関
節マニピュレータの振動発生を防止しようとすることに
ある。

〔問題点を解決するための手段〕

上記目的は、多関節マニピュレータの現時刻の各関節の
角度を基にし、次の制御サンプリング時刻での各関節の
角度指令値からマニピュレータのアームの姿勢を予測す
ること、この予測したアームの姿勢から次の制御サンプ
リング時に各関節の角度が実現された場合に予想される
各関節にかかるトルクを計算すること、このトルクと各
関節のばね定数から算出される外力の影響を零にするた
めに、必要な各関節の新たな角度を求め、その各角度を
次ステツプでの指令値としてマニピュレータ制御部に出
力することにより達成される。

この問題点を解決するための手段の基本原理を第２図を
用いて説明する。マニピュレータの各関節は、等価的に
第２図に示すような質ｔｍの負荷を持つばね系であると
みなすことができる。

このばね系の運動方程式は式（１）で表わすことができ
る。

ｍｘ＝−ｋ　（ｘ−ｙ）　＋Ｆ　　　　　−（１）ここ
で。

ｍ　：負荷の質量 ！　＋　’！　：外力零の状態からのばねの両側の変位 Ｘ　：変位Ｘの時間に関する二階微分 ｋ　：ばね定数 Ｆ　：外力 １＝０でｆｏのインパクト外力が加わった場合の運動を
考えると、式（１）の初期条件がｘ（０）＝ｙ　（０）
＝Ｏ，ｘ　（０）＝Ｏであるとすると、式（１）の解は
式（２）で表わされるような自由振動を示す。

しかし、１＝０でｆｏが検出可能ならば、ｘ　　（０）
＝Ｏより。

ｆ。

ｙ＝−□　　　　　　　　　　　・・・　（３）とすれ
ば、式（１）からｍｘ＝Ｏとなり振動は発生しないこと
になる。

つまり、予めバネ系の負荷側に加わる外力を予測して、
その外力を打消す力に相当する変位をばねの負荷と反対
側に与える。

第２図のばね系とマニピュレータの関節との対応は、Ｊ
ｉｔ量ｍの負荷が注目する関節に取付けられたアームに
、ばねが関節の減速機に、ばねの支持側が関節のモータ
側に、という関係になると考えることができる。よって
、前述の各関節の角度指令値が第２図のばね系の変位ｙ
に相当し、関節にかかるトルクが第２図のばね系の外力
Ｆに相当する。

〔作用〕

上述の個々の技術的手段の働きについて、第３図に示す
アーム構成を例にとって説明する。通常。

マニピュレータの関節は、直流モータを駆動源として減
速機を介して出力軸であるリンクに接続されている。ま
た、関節の角度を計測するためのパルスエンコーダ等の
センサは、検出感度を上げるためにモータ側に取付ける
のが普通である。この場合のモータ側の関節回転角の指
令値θ（Ｋ）と出力軸側の角度θ′　（Ｋ）との関係は
θ’（Ｋ）＝−θ　（Ｋ）　　　　　　　　・・・　（
４）ここで、 Ｎ：減速比 関節角度の制御は、式（４）におけるθ（Ｋ）を指令値
として間接的にθ′　（Ｋ）を所定値に保持する方式を
とっている。よって、サンプリング時刻Ｋにおける関節
角θ（Ｋ）がら１時刻に＋１における角度指令としてθ
（Ｋ＋１）が出方されるとすると、時刻に＋１でのアー
ムの姿勢は式（４）から予測できる。予測したアームの
姿勢から、第３図の関節６６にかかる負荷トルクの変化
量ΔＴは、 ΔＴ＝θ−Ｗ（ｓｉｎθ’　（Ｋ　）　−ｓｉｎθ’　
　（Ｋ＋１））・・・　（５） ここで、 Ｑ：リンクの長さ Ｗ：リンクの先端にかかる等偏負荷荷重（重力による荷
重） θ’　（Ｋ）、　θ’　（Ｋ＋１）：　　　　　　　　
　　＼サンプリング時刻Ｋ及びに＋１におけ る出力軸関節角度（重力方向を基準の 零とし１反時計方向を正とする） この負荷トルクの変化ΔＴに対応して出力軸の角度変化
を生じさせないためのモータ側の角度補正量へ〇（Ｋ＋
１）は 八Ｔ Δθ（Ｋ＋１）＝−□　　　　　・・・　（６）α ここで、 α：関節の入出力軸間のばね定数 このようにして推定した角度指令の補正値と補正前の指
令値から新たな指令値は、 θ（Ｋ＋１）＝θ（Ｋ）＋Δθ　　・・・　（７）これ
らの動作によって、マニピュレータの姿勢変化によって
発生する各関節の負荷変化に伴う振動発生要因を無くす
ことができる。

〔実施例〕

以下、本発明の一実施例を第１図及び第４図ないし第８
図により説明する。

第１図は本発明による振動防止装置を大関節のマニピュ
レータに適用した例を示す１図において、１０はマニピ
ュレータ関節制御部、２０はアーム姿勢演算部、３０は
関節負荷トルク演算部、４０は関節角度補正値演算部、
５０はドライバ回路、６０は多関節マニピュレータであ
る。マニピュレータ関節制御部１０では、多関節マニピ
ュレータ６０の各関節の角度指令値を演算して各制御サ
ンプリングごとにドライバ回路５０へ出力する機能をも
つ、多関節マニピュレータ６ｏの各関節の角度は、角度
指令値をθ、とすれば、第４図に示す位置サーボ系によ
って制御することができる。第４図において、５１は比
例と積分の要素によって構成される制御系、５２は関節
の動特性を示す伝達関数、５３は関節の角度を検出する
ために取付けたエンコーダの値から関節角度へ変換する
ための定数である。なお、制御系５１でＫｐは比例ゲイ
ン、Ｋ！は積分ゲインを示す。

第１図及び第４図に示した本発明の一実施例を第５図に
示す多関節マニピュレータを例にさらに詳しく説明する
。第５図において、６４ａ〜６４ｅはリンク、６６ａ〜
６６ｅは関節、６７はハンドを示す、また、Ｑｉ〜Ｑ８
は各関節間の長さ、０２′〜０６′は関節６６ｂ〜６６
ａの角度（垂直方向を基準とし、反時計方向を正とする
）である。

多関節マニピュレータは、一般に、第５図６６ａに示す
ように、リンク６４ａの軸回りに回転運動するロールと
６６ｂに示すようにリンクを上・下に操作できるような
ピッチによって構成されている。ここでは、第５図に示
す五個の関節により構成するモニビュレータについて説
明する。

マニピュレータの姿勢制御は、各制御ステップごとに出
力する各関節の角度指令値に基づいて制御系を介して実
現される。よって、第１図に示すマニピュレータ関節制
御部１０からは、各制御ステップごとに、ドライバ回路
５０に関節６６ａ〜６６ｓに対応する関節角度指令値θ
Ｊ　　（Ｋ＋１）を出力し、ドライバ回路５０内に組込
まれた第４図に示す制御系により関節を制御する。

第６図から第８図に示すフローチャートは、第１図に示
す各ブロックでの処理内容を示したものである。まず、
第１図に示すアーム姿勢演算部２０の処理内容を第６図
を用いて説明する。第一の処理内容２１は、マニピュレ
ータ関節制御部１０から、決定された制御ステップ（Ｋ
＋１）での各関節６６ａ〜６６ｅの角度指令値θＪ（Ｋ
＋１）（ｊ＝１〜５）及び各関節に接続されたリンク６
４ａ〜６４ａの長さθ−（ｊ＝１〜５）を入力すること
を示す、第二の処理内容２２は、入力した角度指令値θ
Ｊ（Ｋ＋１）及びリンクの長さＱａから、各関節の負荷
トルク算出に必要な重力方向と直角な成分のリンク長さ
ｈＪ　　（Ｋ＋１）（ｊ＝１〜５）を式（８）によって
演算することを示している。

ｈ　Ｊ（Ｋ　＋　　１　）＝　　Ｑ　Ｊ　　・　ｓｕｎ
　θノ（Ｋ　＋　　１　）／　ＮＪ・・・（８） （ｊ＝１〜５） ここで、Ｎｊ　：第ｊ関節の減速化 第三の処理内容２３は、式（８）によって演算した値ｈ
　ａ　　（Ｋ　＋　１　）　　（ｊ　＝　１〜５）をメ
モリに格納すること゛を示している。

次に第１図に示す関節負荷トル演算部３０の処理内容を
第７図を用いて説明する。第７図の第１の処理内容３１
は、制御ステップ（Ｋ＋１）でのマニピュレータで予測
される各関節にかかる負荷トルクＴ、（Ｋ＋１）を式（
９）によって演算することを示している。第二の処理内
容３２は、・・・　（９） （ｊ＝１〜５） ここで、 Ｗ愈　：第ｉ関節のリクンＱ１の先端にかかる等価負荷
荷重 求めた制御ステップ（Ｋ＋１）での各関節にかかると予
測される負荷トルクＴＪ　（Ｋ＋１）と制御ステップに
で各関節にかかつていた負荷トルクＴＪ　　（Ｋ）との
差である負荷トルクの変動分ΔＴＪ（Ｋ＋１）　　を式
（１０）によって演算することを示している。

ΔＴＪ　　（Ｋ＋　１）＝Ｔａ　　（Ｋ＋　１）　　　
Ｔａ　　（Ｋ）・・・　（１０） さらに、第１図に示す関節角度補正値演算部４０の処理
内容を第８図を用いて説明する。第８図の第一の処理内
容４１は、各関節のばね定数αａ（ｊ＝１〜５）と負荷
トルク変動分ΔＴ。

（Ｋ＋１）（ｊ＝１〜５）より関節角度補正値Δθ−（
Ｋ＋１）　　を式（１１）により演算することを示して
いる。第二の処理内容４２は、制御ステラΔθＪ（Ｋ＋
１）＝−ΔＴＪ　（Ｋ＋１）／α。

・・・　（１１） （ｊ＝１〜５） ブ（Ｋ＋１）での負荷トルクの変動分を考慮した補正後
の関節角度指令値θｊ（Ｋ＋１）　　を式（１２）によ
って演算し、第１図のマニピュレータ関節制御部１０に
出力することを示している。

θＪ（Ｋ＋１）＝θ、（Ｋ＋１）＋Δθ、（Ｋ＋１）・
・・　（１２） （ｊ＝１〜５） なお、第６図ないし第８図に示す演算回路はマイクロコ
ンピュータによっても実現できる。

〔発明の効果〕

本発明によれば、振動の発生を防止できるので。

マニピュレータの振動の発生に伴う手先の位置決めの難
しさを解決できる。

【図面の簡単な説明】

第１図は本発明の一実施例の多関節マニピュレータの振
動防止装置の一例を示す図、第２図は本発明の詳細な説
明図、第３図は本発明を一個の関節に適用した場合の説
明図、第４図は実施例の五自由度の多関節マニピュレー
タの構成図、第５図ないし第８図は第１図の各処理演算
部の処理内容を具体的に示すフローチャートである。 １０・・・マニピュレータ関節制御部、２０・・・アー
ム姿勢演算部、３０・・・関節負荷トルク演算部、４０
・・・関節角度補正値演算部、５０・・・ドライバ回路
。 ６０・・・多関節マニピュレータ、６４・・・リンク、
６６・・・関節、６７・・・ハンド、１００・・・ばね
、１１０・・・負荷。

Claims (1)

1. 【特許請求の範囲】 １、複数の関節より構成される多関節マニピュレータに
   おいて、 駆動関節の次制御ステップでの角度指令値によるマニピ
   ュレータの現時点との姿勢変化から各関節に加わるトル
   クの変化量を推定する手段と、前記各関節に生じるトル
   ク変化量と前記各関節のばね定数から前記各関節の角度
   変化を算出する手段と、この算出値に応じて前記各関節
   の前記角度指令値を変更する手段を設けたことを特徴と
   する多関節アームの振動防止装置。