JPH0224076A - 位置決めの補償装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 ［産業上の利用分野］ 本発明はマニピュレータ等の物体取扱い装置における位
置決め制御系に係り、特に重力等の一定方向の力による
位置決め誤差を補償して必要な高精度の位置決めを行う
制御系に好適な位置決めの補償装置に関する。

［従来の技術］ 従来のロボットマニピュレータにおける重力および外力
補償手段は、例えばアール・ピー・ポール（Ｒ−Ｐ−Ｐ
ａｕｌ）著、吉川恒夫訳「ロボットマニピュレータ」コ
ロナ社、昭和６０年７月３０日発行の特に第１５２頁か
ら第１８８頁の第６章、動力学において論じられている
。

［発明が解決しようとする課題］ 上記従来技術は、重力を含む動特性の厳密解が求まるも
のの演算量が極めて多く、計算機への負担が大きいため
サンプリング間隔が長くなって、応答速度が遅くなって
しまう問題と、また状況の変化に弱くて位置決め装置（
ロボット）および対象物の質量等の変化があれば、変化
したパラメータを全て制御装置に入力する必要があると
いう問題があった。

本発明の目的は重力等の一定方向の力が作用する位置決
め制御系において、少ない演算量でしかも状況の変化に
柔軟に対応して精度よく位置決めを行うことのできる位
置決めの補償装置を提供するにある。

［課題を解決するための手段］ 上記目的は、位置決め装置および位置決めされる対象物
の少なくとも一方に重力等による一定方向の力が作用し
ている場合の位置決め制御系において、制御系に与える
目標値として真の目標値に対して一定方向の力と逆方向
のあるオフセットを加えたものを用いるようにした位置
決めの補償装置により達成され、そのオフセットの値を
ファジー関数で与えることもでき、またオフセットの値
を決定するための位置情報を画像を利用することによっ
て得ることもできる。

［作用コ 上記位置決めの補償装置は、重力等の一定方向の外力が
作用する制御系において、入力する目標値と現在の位置
との差分に比例した指定値が外力とつり合った時に位置
決めされる対象物が停止するから、したがって停止位置
すなわち現在の位置が入力する目標値とは一致しないの
で、このずれ分を見こして真の目標値にオフセットを加
えたものを入力する目標値として与えることにより、真
の目標値と現在位置を一致させることができ、そのオフ
セットの値をファジー関数で与えることもでき、またオ
フセットの値を決定するための位置情報を画像を利用し
て得ることも可能である。

［実施例］ 以下に本発明の実施例を第１図ないし第８図により説明
する。

第２図は本発明による位置決めの補償装置の一実施例を
示すロボットのシステム構成図である。

第２図において、２０１はロボット全体を制御する中央
演算装置、２０２はＤ／Ａ変換器、２０３はパワーアン
プ、２０４〜２０９は６箇の関節を有するロボットの第
１〜第６関節駆動装置、２１０は物体を把持する把持部
を駆動する把持部駆動装置、２１１〜２１６は第１〜第
６関節に設けられた第１〜第６関節角検出器、２１７は
第１〜第６関節角検出器２１１〜２１６の検出結果を中
央演算装置２０１へ入力するディジタル入力器である。

この構成で、中央演算装置２０１で決定した第１〜第６
関節駆動装置２０４〜２０９および把持部駆動装置２１
０への出力値はＤ／Ａ変換器２０２によりアナログ信号
に変換され、パワーアンプ２０３を経て第１〜第６関節
駆動装置２０４〜２０９および把持部駆動装置２１０を
駆動する。なお以下各図面を通じて同一符号または記号
は同一または相当部分を示すものとする。

第３図は第２図のロボットのマニピユレータ部の外観図
である。第３図において、３０１〜３０６はマニピユレ
ータ部の６箇の第１〜第６関節、３０７は把持部である
。第３図のマニピユレータ部の第１〜第６関節３０１〜
３０６にはマニピュレータを各軸回りに回転させるため
の第１〜第６関節駆動装置２０４〜２０９および各種信
号線を内蔵しており、第１〜第６関節３０１〜３０６に
は各関節角度を検出するための第１〜第６関節角検出器
２１１〜２１６が取り付けである。マニピユレータ部の
把持部３０７は把持部駆動装置２１０により駆動される
。このマニピュレータ先端の把持部３０７の基準位置の
位置決め制御を中央演算装置２０１によって行う。

第４図は第２図のロボットの１関節のモデル構成図であ
る。第４図において、４０１はプーリ、４０２は質点で
ある。つぎに制御系の構成を説明するにあたり、本発明
による位置決めの補償装置の原理と概念を簡単に説明す
るために、第２図に示す１つの関節のみを取り出したモ
デルにより、まず１つの関節のみの場合について説明す
る。第４図の１つの関節駆動装置２０４はプーリ４０１
を駆動して質点４０２を重力に逆って巻き上げる。この
ときの位置は関節角検出器２１１により検出し、ディジ
タル入力器２１７によりディジタル化されて中央演算袋
＠２０１に入力される。中央演算装置２０１はこの入力
に基づいて指定値を出力し、その指定値はＤ／Ａ変換器
によりアナログ信号に変換され、パワーアンプ２０３を
経て関節駆動装置２０４を駆動する。

つぎにこの制御系の定式化を行う。いま関節駆動装置２
０４の目標値をθ１、関節角検出器２１１の検出した現
在の角度をθ。とじ、パワーアンプ２０３のゲインをＫ
ａとすると、関節駆動装置２０４に与える電圧Ｖ　Ｐｗ
Ｍは次式で与えられる。

ＶｐｗＭ＝Ｋａ（θ、−〇、　）　　　　　　　　　（
１）つぎに関節駆動装置２０４における電圧ｖ　ｐｗＭ
と電流ｉとの関係は次式で与えられる。

ここでに、は逆起電力定数、ωは関節駆動装置２０４の
角速度、Ｒは関節駆動装置２０４の内部抵抗、Ｌは関節
駆動装置２０４のインダクタンスである。つぎに関節駆
動装置２０４の出力トルクをτ、トルク定数をＫｔとす
ると、次式を与える。

τ＝Ｋｔｉ　　　　　　　　　　　　　　　（３）また
関節駆動装置２０４の慣性モーメントとプーリ４０１の
慣性モーメントの和をＪ、関節駆動装置２０４の粘性抵
抗定数をＢ、質点４０２の質量をｍ、プーリ４０１の半
径をｒ、重力加速度をｇとすると１次の運動方程式が得
られる。

またωとθ。の関係は次式の通りである。

上記の（１）〜（５）式をラプラス変換することにより
次式を得る。

Ｖ　ｐｗＭ＝Ｋａ　（θ’ｌ−１３’０）　　　　　　
　（１）’ｖ　ｐｙＨ＝に６ω’＋Ｒｉ’＋Ｌｓ　ｉ　
　　　　（２）’τ　＝　Ｋｔｉ　’　　　　　　　　
　　　　　（３）’τ＝Ｊ　Ｓ　（Ｌｌ’＋Ｂ　（ｌｌ
’＋　Ｉｌｌｇｒ（４）’ω　＝Ｓθ′。　　　　　　
　　　　　　　　　（５）′ここでラプラス変換された
状態変数にはダッシュをつけた。

ここで（１）′〜（５）１式からＶＰＶＭ＋　τ　、１
　、ωを消去して、θ′、とθ′。の関係を求めると次
式をここでθ′、としてステップ入力（＝１／ｓ）を代
入し、十分時間が経った時のθ。はラプラス変換の最終
値定理を用いて次式で与えられる。

この（７）式かられかるように入力に対してＲｍｇｒ　
／ＫａＫｔの偏差が残る。本発明による位置決めの補償
装置はこの偏差をＯに近づけ、より精度のよい制御を行
うことを目的とする。

第５図は第４図の制御系のブロック線図である。

第５図において、１点鎖線内部を除く部分が上記の（１
）′〜（５）１式をブロック線図で表わしたもので、５
０２はθ’、、５０３はＶ　ｐｗｘ、５０４はｉ、５０
５はτ５０６はω　、５０７はθ′。にそれぞれ対応し
、５０８は（４）１式の右辺第３項で重力によるトルク
を表わす。

第５図の５０８の１点鎖線内部が本発明による位置決め
の補償の論理を示したもので、５０９はホールダである
。この本発明による補償の論理５０８は目標値５０２と
して上記（６）式のθ′、をそのまま与えるのでなく、
真の目標値５０１（θ′、ｒ）と現在の角度５０７（θ
′。）の差分を目標値にオフセットとして加え、その値
を目標値５０２（θ′、）とする。つまり偏差分を見込
んで目標値５０２（θ′１）を設定し直すことになる。

つぎにこの位置決めの補償の論理５０８の概念を定式化
する。いま真の目標値５０１をＯ’ｉｒとする。

補償の論理５０８のホールダ５０９およびホールダ回り
のフィードバックは連続系においては積分項１／ｓで置
き換わるので、目標値５０２（θ′、）は次式で与えら
れる。

θ′、＝−！−（θｉｒ−〇’、＞（８）（８）式を（
６）式に代入し、θ′０についてまとめると次式を得る
。

ここでθ　ｉｒとしてステップ入力（＝１／Ｓ）を代入
し、十分時間が経った時のＯ８はラプラス変換の最終値
定理により次式で与えられる。

＝　１　　　　　　　　　　　　　　　　　　　　　　
　　　（１０）上記に示した通りに制御論理に動特性を
考慮しなくても偏差は０となった。以上の１つの関節の
みの場合について説明した概念を第２図のロボットのシ
ステム全体に適用することができる。

第１図は本発明による位置決めの補償装置の一実施例を
示すロボットの制御アルゴリズムのブロック線図である
。第１図において、上記した第５図の位置決めの補償論
理５０８の概念を第２図のロボットのシステム全体に適
用したブロック線図を示し、このブロック線図は第２図
の中央演算装置２０１の内部における処理を表わすもの
である。第１図の１０１は目標値発生論理、１０２はラ
プラス逆変換、１０３は位置決めの補償論理、１０５〜
１１０はマニピュレータ先端の把持部３０７の姿勢・位
置の目標値、１１１〜１１６は第１〜第６関節角目橿値
、１１７〜１２２は第１〜第６関節指令値、１２３〜１
２８は第１〜第６関節の現在の関節角である。

この制御系の構成で、中央演算装置２０１内の目標値発
生論理１０１によりマニピュレータ先端の把持部３０７
の姿勢・位置の目標値１０５〜１１０が決定される。こ
の姿勢・位置の目標値１０５〜１１０は姿勢の目標値１
０５〜１０７のロール角の目標値１０５、ピッチ角の目
標値１０６、ヨー角の目標値１０７と、位置の目標値１
０８〜１１０の前後方向の位置の目標値１０８、左右方
向の位置の目標値１０９、上下方向の位置の目標値１１
０を含む、なお上下方向の位置の目標値１１０は第５図
の位置決めの補償論理５０８に対応する補償論理１０３
で補正される。これらの目標値１０５〜１１０を受けて
ラプラス逆変換１０２で逆変換され、マニピュレータの
第１〜第６関節３０１〜３０６の関節角目標値１１１〜
１１６を求める。これらの第１〜第６関節角目標値１１
１〜１１６からマニピュレータの第１〜第６関節３０１
〜３０６の第１〜第６関節角検出器２１１〜２１６で検
出された現在の関節角１２３〜１２８が減じられ、それ
らの第１〜第６関節３０１〜３０６の第１〜第６関節駆
動装置２０４〜２０９への指令値１１７〜１２２が中央
演算装置２０１から出力される。また中央演算装置２０
１へ出力したマニピュレータの第１〜第６関節３０１〜
３０６の現在の関節角１２３〜１２８は上記のフィード
バックの減算に使用される他に、ラプラス正変換１０４
で正変換を行って第５図の現在の角度５０７（θ′。）
に対応するマニピュータの上下方向の位置１２９を得、
この値が補償論理１０３ヘフイードバツクされる。この
フィードバックにより重力によるマニピュレータ先端の
把持部３０７の上下方向の変位が補正される。

第６図は本発明による位置決めの補償装置の他の実施例
を示すロボットの制御アルゴリズムのブロック線図であ
る。第６図において、ファジー関数によるファジー制御
を用いて目標値を補正する位置決めの補償装置の例を示
し、６０１はファジールール、６０２〜６０７はファジ
ールール６０１により求めた姿勢・位置の目標値１０１
〜１１０への補正値である。第６図の第１図との違いは
目標値発生回路１０１と逆変換１０２の間の姿勢・位置
の目標値１０５〜１１０の補正を第１図の現在のマニピ
ュレータの上下方向位置１２９を用いて補償論理１０３
で上下方向の位置の目標値１１０を補正することにより
行うのではなく、姿勢・位置の目標値１０５〜１１０の
補正をファジールール６０１で求めた姿勢・位置の目標
値１０５〜１１０への補正値６０２〜６０７により行っ
ている点である。なおこの制御系のハードウェアのシス
テム構成は第１図の実施例と同じく第２図および第３図
に示すロボットのシステムである。

つぎにこの構成のファジールール６０１について説明す
る。このファジーモデルとして次の３つのプラント法則
をとるものとする。

ｖ：ｓｆｎ工＋Ｙ’　ｉｓ　Ｂｉｇ　ｔｈｅｎ　Ｙ’Ｈ
＝　ｆ　：Ｌ’　：　ｉｆ　５ア「イｉｓ　Ｓｍａｌｌ
　ａｎｄ　Ｚ　ｉｓ　Ｂｉｇｔｈｅｎ　Ｙｊ＝　　ｆ　
７ Ｌ：ｌ：　ｉｆ　％　ｉｓ　Ｓｍａｌｌ　ａｎｄ　Ｚ　
ｉｓ　Ｓｍａｌｌｔｈｅｎ　ｙ：＝　　ｆ３．　　　（
ｉ＝１−６）ここでＸはマニピュレータ先端の把持部３
０７の前後方向位置の目標値１０８．　Ｙは左右方向位
置の目標値１０９．２は上下方向位置の目標値１１０で
ある。

また後件部関数ｆ：ｌｆｔ＋ｆ？は経験則による関数で
あってｉ＝１でマニピュレータ先端の把持部３０７の姿
勢ロール角目標値１０５に対する補正値６０２、ｉ＝２
でピッチ角目標値１０６に対する補正値６０３、ｉ＝３
でヨー角目標値１０７に対する補正値６０４、ｉ＝４で
前後方向位置目標値１０８に対する補正値６０５、ｉ＝
５で左右方向位置目標値１０９に対する補正値６０６、
ｉ＝６で上下方向位置目標値１１０に対する補正値１０
７を与えるものとする。つぎに５ア「いにおけるファジ
ー集合ｒＢｉｇＪのメンバシップ関数をＡ、、Ｊｘ２＋
ｙ２におけるファジー集合ｒ　Ｓ　ｍａｌｌ　Ｊのメン
バシップ関数をＡｔ、Ｚにおけるファジー集合ｒＢｉｇ
Ｊのメンバシップ関数をＡ３、Ｚにおけるファジー集合
ｒｓｍａｌｌＪのメンバシップ関数をＡ４とすると、各
ｉ＝１〜６の目標値１０５〜１１０に対する補正値６０
２〜６０７の補正値ｍＨ（ｉ＝１〜６）は次式で与えら
れる。

第７図は本発明による位置決めの補償装置のさらに他の
実施例を示すロボットの制御アルゴリズムのブロック線
図である。第７図において、画像情報を用いて目標値を
補正する位置決めの補償装置の例を示し、７０１は画像
情報解析論理、７０２は画像情報解析論理７０１からの
上下方向位置７０２，７０３は画像信号である。また第
８図は第７図のロボットのシステム構成図である。第８
図において、８０１は画像入力装置、８０２は画像処理
装置である。第８図の第２図との違いは画像入力装置８
０１と画像処理装置８０２を追加した点で、マニピュレ
ータ先端の把持部３０７の部分の様子は画像入力装置８
０１で入力し、画像処理装置８０２で画像処理したのち
、中央演算装置２０１ヘマニピユレータ先端の把持部３
０７の上下方向位置７０３の信号として取り込む。この
ときの第７図の中央演算装置２０１内部での制御アルゴ
リズムのブロック線図の第１図との違いは、第１図では
位置決めの補償論理１０３へのフィードバック信号とし
て第１〜第６関節の現在の関節角１２３〜１２８から正
変換１０４で正変換して得た現在のマニピュレータ先端
の把持部３０７の上下方向位置１２９を用いているのに
対して、第７図では第１図の上下方向位置１２９の代り
に該上下方向位置１２９に対応するものとして、画像処
理装置８０２からの画像信号７０３を画像情報解析論理
７０１で解析して得られる現在のマニピュレータ先端の
把持部３０７の上下方向位置７０２の信号を補償論理１
０３へのフィードバック信号としていることである。こ
の実施例によれば、実際のマニピュレータ先端の把持部
３０７の上下方向の現在位置を補償論理１０３ヘフイー
ドバツクしているので、マニピユレータ部のアームのた
わみ等にも対応できる。

以上の実施例によれば、位置決め装置の制御系における
位置決めの補償手段で補正を行うことにより、目標値を
操作してマニピュレータ先端の把持部の重力等による変
位を補正できる。なお上記の実施例は重力の作用するロ
ボットシステムの位置決め制御系について説明したが、
本発明は一定方向の力が位置決め装置および位置決めさ
れる対象物の少なくとも一方に作用する場合の位置決め
制御系に同様に適用可能である。

［発明の効果］ 本発明によれば、位置決め装置および位置決めされる対
象物の少なくとも一方に一定方向のカが作用する位置決
め制御系において、大量な演算を施さなくても簡易に一
定方向のカによる対象物の変位量を補正できるので、制
御系の設計が簡単になるうえ計算機の負担軽減に基づく
応答性の向上がもたらされ、精度よい位置決め制御を行
えるという効果がある。

【図面の簡単な説明】

第１図は本発明による位置決めの補償装置の一実施例を
示すブロック線図、第２図は第１図のシステム構成図、
第３図はマニピュレータ外観図、第４図は第１図の１関
節のみのモデル構成図、第５図は第４図のブロック線図
、第６図は本発明による他の実施例を示すブロック線図
、第７図は本発明によるさらに他の実施例を示すブロッ
ク線図、第８図は第７図のシステム構成図である。 １０１・・・目標値発生論理、１０２・・・ラプラス逆
変換、１０３・・位置決めの補償論理、１０４・・・ラ
プラス正変換、１０５〜１１０・・姿勢・位置の目標値
、１１１〜１１６・・・関節角目標値、１１７〜１２２
・・・関節指令値、１２３〜１２８・・・現在の関節角
、１２９・・・現在の上下方向位置、２０１・・・中央
演算装置、２０２・・・Ｄ／Ａ変換器、２０３・・・パ
ワーアンプ、２０４〜２０９・・・関節駆動装置、２１
０・・・把持部駆動装置、２１１〜２１６・・・関節角
検出器、２１７・・・ディジタル入力器、３０１〜３０
６・・・関節、３０７・・・把持部、５０８・・・位置
決め補償論理、６０１・・・ファジールール、７０１・
・・画像情報解析論理、７０２・・・現在の上下方向位
置、７０３・・・画像信号、８０１・・・画像入力装置
、８０２・・・画像処理装置。 代理人　弁理士　　秋　本　正　実 第 ＼、　　　　　　　　　□又２１Ｑ 図 ３０１〜３０６第１〜第６関節 ３０７ｚ乃仰 第６図

Claims (1)

1. 【特許請求の範囲】 １、位置決め装置および位置決めされる対象物の少なく
   とも一方に一定方向の力が作用している場合の位置決め
   制御系において、真の目標値に対して上記一定方向の力
   の向きと逆方向にオフセットを加えたものを目標値とし
   て与える手段を備えたことを特徴とする位置決めの補償
   装置。 ２、上記オフセットの値をファジー関数で与えることを
   特徴とする請求項１記載の位置決めの補償装置。 ３、上記オフセットの値を決定するための対象物の位置
   情報を画像を利用することによって得ることを特徴とす
   る請求項１記載の位置決めの補償装置。 ４、上記一定方向の力が重力である場合の請求項１記載
   の位置決めの補償装置。