JPH0224076A - Positioning compensating device - Google Patents

Abstract

PURPOSE:To lighten the load applied onto a calculator and improve responsiveness by installing a means which provides the value which is obtained by adding offset in the reverse direction to the direction of a force in a certain direction onto a true aimed value, as an aimed value, and by permitting the displacement quantity of an objective article due to the force in a certain direction to be corrected in simple manners without carrying out a large quantity of calculations. CONSTITUTION:In a control system in which an external force in a certain direction such as gravity acts, an objective article which is to be positioned stops at a time point when a designated value in proportion to the difference between an inputted aimed value and the position at present is balanced with the external force. Taking account of this difference, a positioning compensation logic 103 provides the value which is obtained by adding offset onto a true aimed value, as an aimed value to be inputted. Therefore, the true aimed value and the position at present can be allowed to coincide with each other.

Description

【発明の詳細な説明】 ［産業上の利用分野］ 本発明はマニピュレータ等の物体取扱い装置における位
置決め制御系に係り、特に重力等の一定方向の力による
位置決め誤差を補償して必要な高精度の位置決めを行う
制御系に好適な位置決めの補償装置に関する。[Detailed Description of the Invention] [Industrial Application Field] The present invention relates to a positioning control system in an object handling device such as a manipulator, and in particular compensates for positioning errors caused by forces in a fixed direction such as gravity to achieve the necessary high precision. The present invention relates to a positioning compensation device suitable for a control system that performs positioning.

［従来の技術］ 従来のロボットマニピュレータにおける重力および外力
補償手段は、例えばアール・ピー・ポール（Ｒ−Ｐ−Ｐ
ａｕｌ）著、吉川恒夫訳「ロボットマニピュレータ」コ
ロナ社、昭和６０年７月３０日発行の特に第１５２頁か
ら第１８８頁の第６章、動力学において論じられている
。[Prior Art] Gravity and external force compensating means in conventional robot manipulators are, for example, those of R-P-P.
This is particularly discussed in Chapter 6, Dynamics, from pages 152 to 188 of "Robot Manipulator" by Robert Aul, translated by Tsuneo Yoshikawa, published by Corona Publishing, July 30, 1985.

［発明が解決しようとする課題］ 上記従来技術は、重力を含む動特性の厳密解が求まるも
のの演算量が極めて多く、計算機への負担が大きいため
サンプリング間隔が長くなって、応答速度が遅くなって
しまう問題と、また状況の変化に弱くて位置決め装置（
ロボット）および対象物の質量等の変化があれば、変化
したパラメータを全て制御装置に入力する必要があると
いう問題があった。[Problems to be Solved by the Invention] Although the above-mentioned conventional technology can find exact solutions for dynamic characteristics including gravity, the amount of calculation is extremely large and the burden on the computer is large, resulting in long sampling intervals and slow response speed. The positioning device (
There is a problem in that if there is a change in the mass, etc. of the robot (robot) or the object, all the changed parameters must be input to the control device.

本発明の目的は重力等の一定方向の力が作用する位置決
め制御系において、少ない演算量でしかも状況の変化に
柔軟に対応して精度よく位置決めを行うことのできる位
置決めの補償装置を提供するにある。An object of the present invention is to provide a positioning compensation device that can perform accurate positioning in a positioning control system where forces in a fixed direction, such as gravity, act, with a small amount of calculations, while flexibly responding to changes in the situation. be.

［課題を解決するための手段］ 上記目的は、位置決め装置および位置決めされる対象物
の少なくとも一方に重力等による一定方向の力が作用し
ている場合の位置決め制御系において、制御系に与える
目標値として真の目標値に対して一定方向の力と逆方向
のあるオフセットを加えたものを用いるようにした位置
決めの補償装置により達成され、そのオフセットの値を
ファジー関数で与えることもでき、またオフセットの値
を決定するための位置情報を画像を利用することによっ
て得ることもできる。[Means for Solving the Problem] The above object is to provide a target value to the control system in a positioning control system when a force in a fixed direction due to gravity or the like is acting on at least one of the positioning device and the object to be positioned. This is achieved by a positioning compensation device that uses a force in a constant direction and a certain offset in the opposite direction to the true target value.The offset value can also be given by a fuzzy function, and the offset Position information for determining the value of can also be obtained by using images.

［作用コ 上記位置決めの補償装置は、重力等の一定方向の外力が
作用する制御系において、入力する目標値と現在の位置
との差分に比例した指定値が外力とつり合った時に位置
決めされる対象物が停止するから、したがって停止位置
すなわち現在の位置が入力する目標値とは一致しないの
で、このずれ分を見こして真の目標値にオフセットを加
えたものを入力する目標値として与えることにより、真
の目標値と現在位置を一致させることができ、そのオフ
セットの値をファジー関数で与えることもでき、またオ
フセットの値を決定するための位置情報を画像を利用し
て得ることも可能である。[Operation] In a control system where an external force such as gravity acts in a certain direction, the positioning compensation device described above is positioned when a specified value proportional to the difference between the input target value and the current position balances the external force. Since the object stops, the stop position, that is, the current position, does not match the input target value, so take this deviation into consideration and give the true target value plus the offset as the input target value. This allows the true target value to match the current position, and the offset value can be given using a fuzzy function, and it is also possible to use images to obtain position information for determining the offset value. It is.

［実施例］ 以下に本発明の実施例を第１図ないし第８図により説明
する。[Example] Examples of the present invention will be described below with reference to FIGS. 1 to 8.

第２図は本発明による位置決めの補償装置の一実施例を
示すロボットのシステム構成図である。FIG. 2 is a system configuration diagram of a robot showing an embodiment of the positioning compensation device according to the present invention.

第２図において、２０１はロボット全体を制御する中央
演算装置、２０２はＤ／Ａ変換器、２０３はパワーアン
プ、２０４〜２０９は６箇の関節を有するロボットの第
１〜第６関節駆動装置、２１０は物体を把持する把持部
を駆動する把持部駆動装置、２１１〜２１６は第１〜第
６関節に設けられた第１〜第６関節角検出器、２１７は
第１〜第６関節角検出器２１１〜２１６の検出結果を中
央演算装置２０１へ入力するディジタル入力器である。In FIG. 2, 201 is a central processing unit that controls the entire robot, 202 is a D/A converter, 203 is a power amplifier, 204 to 209 are first to sixth joint drive devices of the robot having six joints, Reference numeral 210 denotes a gripping unit driving device that drives a gripping unit that grips an object; 211 to 216 denote first to sixth joint angle detectors provided at the first to sixth joints; and 217, first to sixth joint angle detection units. This is a digital input device that inputs the detection results of the devices 211 to 216 to the central processing unit 201.

この構成で、中央演算装置２０１で決定した第１〜第６
関節駆動装置２０４〜２０９および把持部駆動装置２１
０への出力値はＤ／Ａ変換器２０２によりアナログ信号
に変換され、パワーアンプ２０３を経て第１〜第６関節
駆動装置２０４〜２０９および把持部駆動装置２１０を
駆動する。なお以下各図面を通じて同一符号または記号
は同一または相当部分を示すものとする。With this configuration, the first to sixth
Joint drive devices 204 to 209 and grip drive device 21
The output value to 0 is converted into an analog signal by the D/A converter 202, and then passes through the power amplifier 203 to drive the first to sixth joint drive devices 204 to 209 and the grip portion drive device 210. Note that the same reference numerals or symbols indicate the same or corresponding parts throughout the drawings.

第３図は第２図のロボットのマニピユレータ部の外観図
である。第３図において、３０１〜３０６はマニピユレ
ータ部の６箇の第１〜第６関節、３０７は把持部である
。第３図のマニピユレータ部の第１〜第６関節３０１〜
３０６にはマニピュレータを各軸回りに回転させるため
の第１〜第６関節駆動装置２０４〜２０９および各種信
号線を内蔵しており、第１〜第６関節３０１〜３０６に
は各関節角度を検出するための第１〜第６関節角検出器
２１１〜２１６が取り付けである。マニピユレータ部の
把持部３０７は把持部駆動装置２１０により駆動される
。このマニピュレータ先端の把持部３０７の基準位置の
位置決め制御を中央演算装置２０１によって行う。FIG. 3 is an external view of the manipulator section of the robot shown in FIG. 2. In FIG. 3, 301 to 306 are six first to sixth joints of the manipulator section, and 307 is a gripping section. 1st to 6th joints 301 of the manipulator part in FIG.
306 has built-in first to sixth joint drive devices 204 to 209 and various signal lines for rotating the manipulator around each axis, and the first to sixth joints 301 to 306 have built-in devices that detect each joint angle. The first to sixth joint angle detectors 211 to 216 are attached. The gripping portion 307 of the manipulator portion is driven by a gripping portion driving device 210. The central processing unit 201 controls the positioning of the reference position of the gripping portion 307 at the tip of the manipulator.

第４図は第２図のロボットの１関節のモデル構成図であ
る。第４図において、４０１はプーリ、４０２は質点で
ある。つぎに制御系の構成を説明するにあたり、本発明
による位置決めの補償装置の原理と概念を簡単に説明す
るために、第２図に示す１つの関節のみを取り出したモ
デルにより、まず１つの関節のみの場合について説明す
る。第４図の１つの関節駆動装置２０４はプーリ４０１
を駆動して質点４０２を重力に逆って巻き上げる。この
ときの位置は関節角検出器２１１により検出し、ディジ
タル入力器２１７によりディジタル化されて中央演算袋
＠２０１に入力される。中央演算装置２０１はこの入力
に基づいて指定値を出力し、その指定値はＤ／Ａ変換器
によりアナログ信号に変換され、パワーアンプ２０３を
経て関節駆動装置２０４を駆動する。FIG. 4 is a model configuration diagram of one joint of the robot shown in FIG. 2. In FIG. 4, 401 is a pulley and 402 is a mass point. Next, in order to explain the configuration of the control system, in order to simply explain the principle and concept of the positioning compensation device according to the present invention, we will start by using a model in which only one joint is taken out, as shown in FIG. The case will be explained below. One joint drive device 204 in FIG.
is driven to wind up the mass point 402 against gravity. The position at this time is detected by the joint angle detector 211, digitized by the digital input device 217, and input to the central calculation bag @201. The central processing unit 201 outputs a designated value based on this input, and the designated value is converted into an analog signal by a D/A converter and drives the joint driving device 204 via the power amplifier 203.

つぎにこの制御系の定式化を行う。いま関節駆動装置２
０４の目標値をθ１、関節角検出器２１１の検出した現
在の角度をθ。とじ、パワーアンプ２０３のゲインをＫ
ａとすると、関節駆動装置２０４に与える電圧Ｖ　Ｐｗ
Ｍは次式で与えられる。Next, we will formulate this control system. Now joint drive device 2
04 target value is θ1, and the current angle detected by the joint angle detector 211 is θ. and set the gain of the power amplifier 203 to K.
a, the voltage applied to the joint drive device 204 V Pw
M is given by the following equation.

ＶｐｗＭ＝Ｋａ（θ、−〇、　）　　　　　　　　　（
１）つぎに関節駆動装置２０４における電圧ｖ　ｐｗＭ
と電流ｉとの関係は次式で与えられる。VpwM=Ka(θ, −〇, ) (
1) Next, the voltage v pwM in the joint drive device 204
The relationship between the current i and the current i is given by the following equation.

ここでに、は逆起電力定数、ωは関節駆動装置２０４の
角速度、Ｒは関節駆動装置２０４の内部抵抗、Ｌは関節
駆動装置２０４のインダクタンスである。つぎに関節駆
動装置２０４の出力トルクをτ、トルク定数をＫｔとす
ると、次式を与える。Here, is a back electromotive force constant, ω is the angular velocity of the joint drive device 204, R is the internal resistance of the joint drive device 204, and L is the inductance of the joint drive device 204. Next, when the output torque of the joint drive device 204 is τ and the torque constant is Kt, the following equation is given.

τ＝Ｋｔｉ　　　　　　　　　　　　　　　（３）また
関節駆動装置２０４の慣性モーメントとプーリ４０１の
慣性モーメントの和をＪ、関節駆動装置２０４の粘性抵
抗定数をＢ、質点４０２の質量をｍ、プーリ４０１の半
径をｒ、重力加速度をｇとすると１次の運動方程式が得
られる。τ=Kti (3) Also, the sum of the moment of inertia of the joint drive device 204 and the moment of inertia of the pulley 401 is J, the viscous resistance constant of the joint drive device 204 is B, the mass of the mass point 402 is m, the radius of the pulley 401 is r, If the gravitational acceleration is g, a first-order equation of motion is obtained.

またωとθ。の関係は次式の通りである。Also ω and θ. The relationship is as shown in the following equation.

上記の（１）〜（５）式をラプラス変換することにより
次式を得る。The following equation is obtained by Laplace transform of the above equations (1) to (5).

Ｖ　ｐｗＭ＝Ｋａ　（θ’ｌ−１３’０）　　　　　　
　（１）’ｖ　ｐｙＨ＝に６ω’＋Ｒｉ’＋Ｌｓ　ｉ　
　　　　（２）’τ　＝　Ｋｔｉ　’　　　　　　　　
　　　　　（３）’τ＝Ｊ　Ｓ　（Ｌｌ’＋Ｂ　（ｌｌ
’＋　Ｉｌｌｇｒ（４）’ω　＝Ｓθ′。　　　　　　
　　　　　　　　　（５）′ここでラプラス変換された
状態変数にはダッシュをつけた。V pwM=Ka (θ'l-13'0)
(1)'v pyH=6ω'+Ri'+Ls i
(2) 'τ = Kti'
(3)'τ=J S (Ll'+B (ll
'+Illgr(4)'ω=Sθ'.
(5)'Here, state variables that have been Laplace transformed are marked with a dash.

ここで（１）′〜（５）１式からＶＰＶＭ＋　τ　、１
　、ωを消去して、θ′、とθ′。の関係を求めると次
式をここでθ′、としてステップ入力（＝１／ｓ）を代
入し、十分時間が経った時のθ。はラプラス変換の最終
値定理を用いて次式で与えられる。Here, from equations (1)′ to (5) 1, VPVM+ τ , 1
, ω and θ′, and θ′. To find the relationship, use the following equation as θ', substitute step input (=1/s), and obtain θ when sufficient time has passed. is given by the following equation using the final value theorem of Laplace transform.

この（７）式かられかるように入力に対してＲｍｇｒ　
／ＫａＫｔの偏差が残る。本発明による位置決めの補償
装置はこの偏差をＯに近づけ、より精度のよい制御を行
うことを目的とする。From this equation (7), we can see that Rmgr for the input
A deviation of /KaKt remains. The purpose of the positioning compensation device according to the present invention is to bring this deviation closer to O and perform more accurate control.

第５図は第４図の制御系のブロック線図である。FIG. 5 is a block diagram of the control system shown in FIG. 4.

第５図において、１点鎖線内部を除く部分が上記の（１
）′〜（５）１式をブロック線図で表わしたもので、５
０２はθ’、、５０３はＶ　ｐｗｘ、５０４はｉ、５０
５はτ５０６はω　、５０７はθ′。にそれぞれ対応し
、５０８は（４）１式の右辺第３項で重力によるトルク
を表わす。In Fig. 5, the part excluding the area inside the dashed dotted line is
)' to (5) 1 are expressed in a block diagram, and 5
02 is θ', 503 is V pwx, 504 is i, 50
5 is τ506 is ω, and 507 is θ'. , and 508 represents the torque due to gravity in the third term on the right side of equation (4)1.

第５図の５０８の１点鎖線内部が本発明による位置決め
の補償の論理を示したもので、５０９はホールダである
。この本発明による補償の論理５０８は目標値５０２と
して上記（６）式のθ′、をそのまま与えるのでなく、
真の目標値５０１（θ′、ｒ）と現在の角度５０７（θ
′。）の差分を目標値にオフセットとして加え、その値
を目標値５０２（θ′、）とする。つまり偏差分を見込
んで目標値５０２（θ′１）を設定し直すことになる。The area inside the one-dot chain line 508 in FIG. 5 shows the logic of positioning compensation according to the present invention, and 509 is a holder. The compensation logic 508 according to the present invention does not directly give θ' in equation (6) as the target value 502, but
True target value 501 (θ', r) and current angle 507 (θ
'. ) is added to the target value as an offset, and the value is set as the target value 502 (θ', ). In other words, the target value 502 (θ'1) is reset taking into account the deviation.

つぎにこの位置決めの補償の論理５０８の概念を定式化
する。いま真の目標値５０１をＯ’ｉｒとする。Next, we will formulate the concept of this positioning compensation logic 508. Let the true target value 501 be O'ir.

補償の論理５０８のホールダ５０９およびホールダ回り
のフィードバックは連続系においては積分項１／ｓで置
き換わるので、目標値５０２（θ′、）は次式で与えら
れる。Since the holder 509 of the compensation logic 508 and the feedback around the holder are replaced by an integral term 1/s in a continuous system, the target value 502 (θ',) is given by the following equation.

θ′、＝−！−（θｉｒ−〇’、＞（８）（８）式を（
６）式に代入し、θ′０についてまとめると次式を得る
。θ′, =-! -(θir-〇', > (8) Expression (8) is (
6) By substituting into the equation and summarizing θ'0, the following equation is obtained.

ここでθ　ｉｒとしてステップ入力（＝１／Ｓ）を代入
し、十分時間が経った時のＯ８はラプラス変換の最終値
定理により次式で与えられる。Here, a step input (=1/S) is substituted as θ ir, and O8 when sufficient time has elapsed is given by the following equation based on the final value theorem of Laplace transform.

＝　１　　　　　　　　　　　　　　　　　　　　　　
　　　（１０）上記に示した通りに制御論理に動特性を
考慮しなくても偏差は０となった。以上の１つの関節の
みの場合について説明した概念を第２図のロボットのシ
ステム全体に適用することができる。= 1
(10) As shown above, the deviation was 0 even without considering dynamic characteristics in the control logic. The concept described above for the case of only one joint can be applied to the entire system of the robot shown in FIG.

第１図は本発明による位置決めの補償装置の一実施例を
示すロボットの制御アルゴリズムのブロック線図である
。第１図において、上記した第５図の位置決めの補償論
理５０８の概念を第２図のロボットのシステム全体に適
用したブロック線図を示し、このブロック線図は第２図
の中央演算装置２０１の内部における処理を表わすもの
である。第１図の１０１は目標値発生論理、１０２はラ
プラス逆変換、１０３は位置決めの補償論理、１０５〜
１１０はマニピュレータ先端の把持部３０７の姿勢・位
置の目標値、１１１〜１１６は第１〜第６関節角目橿値
、１１７〜１２２は第１〜第６関節指令値、１２３〜１
２８は第１〜第６関節の現在の関節角である。FIG. 1 is a block diagram of a robot control algorithm showing an embodiment of the positioning compensation device according to the present invention. FIG. 1 shows a block diagram in which the concept of the positioning compensation logic 508 in FIG. 5 described above is applied to the entire system of the robot in FIG. It represents internal processing. 1, 101 is the target value generation logic, 102 is the Laplace inverse transform, 103 is the compensation logic for positioning, and 105-
110 is the target value of the posture/position of the gripping part 307 at the tip of the manipulator, 111 to 116 are the first to sixth joint angle values, 117 to 122 are the first to sixth joint command values, and 123 to 1
28 is the current joint angle of the first to sixth joints.

この制御系の構成で、中央演算装置２０１内の目標値発
生論理１０１によりマニピュレータ先端の把持部３０７
の姿勢・位置の目標値１０５〜１１０が決定される。こ
の姿勢・位置の目標値１０５〜１１０は姿勢の目標値１
０５〜１０７のロール角の目標値１０５、ピッチ角の目
標値１０６、ヨー角の目標値１０７と、位置の目標値１
０８〜１１０の前後方向の位置の目標値１０８、左右方
向の位置の目標値１０９、上下方向の位置の目標値１１
０を含む、なお上下方向の位置の目標値１１０は第５図
の位置決めの補償論理５０８に対応する補償論理１０３
で補正される。これらの目標値１０５〜１１０を受けて
ラプラス逆変換１０２で逆変換され、マニピュレータの
第１〜第６関節３０１〜３０６の関節角目標値１１１〜
１１６を求める。これらの第１〜第６関節角目標値１１
１〜１１６からマニピュレータの第１〜第６関節３０１
〜３０６の第１〜第６関節角検出器２１１〜２１６で検
出された現在の関節角１２３〜１２８が減じられ、それ
らの第１〜第６関節３０１〜３０６の第１〜第６関節駆
動装置２０４〜２０９への指令値１１７〜１２２が中央
演算装置２０１から出力される。また中央演算装置２０
１へ出力したマニピュレータの第１〜第６関節３０１〜
３０６の現在の関節角１２３〜１２８は上記のフィード
バックの減算に使用される他に、ラプラス正変換１０４
で正変換を行って第５図の現在の角度５０７（θ′。）
に対応するマニピュータの上下方向の位置１２９を得、
この値が補償論理１０３ヘフイードバツクされる。この
フィードバックにより重力によるマニピュレータ先端の
把持部３０７の上下方向の変位が補正される。With this control system configuration, the target value generation logic 101 in the central processing unit 201 causes the grip portion 307 at the tip of the manipulator to
Target values 105 to 110 of the posture and position of are determined. This posture/position target value 105 to 110 is the posture target value 1
05 to 107 roll angle target value 105, pitch angle target value 106, yaw angle target value 107, and position target value 1
08 to 110, target value 108 for the position in the front and back direction, target value 109 for the position in the left and right direction, target value 11 for the position in the vertical direction
The target value 110 of the vertical position including 0 is the compensation logic 103 corresponding to the positioning compensation logic 508 of FIG.
will be corrected. These target values 105 to 110 are received and inversely transformed by Laplace inverse transformation 102 to obtain joint angle target values 111 to 111 of the first to sixth joints 301 to 306 of the manipulator.
Find 116. These first to sixth joint angle target values 11
1 to 116 to the first to sixth joints 301 of the manipulator
The current joint angles 123 to 128 detected by the first to sixth joint angle detectors 211 to 216 of ~306 are subtracted, and the first to sixth joint drive devices of those first to sixth joints 301 to 306 Command values 117 to 122 to 204 to 209 are output from the central processing unit 201. Also, the central processing unit 20
1st to 6th joints 301 of the manipulator output to 1
In addition to being used for the above feedback subtraction, the current joint angles 123 to 128 of 306 are used for the Laplace positive transformation 104
Perform a positive transformation to obtain the current angle 507 (θ') in Figure 5.
Obtain the vertical position 129 of the manipulator corresponding to
This value is fed back to compensation logic 103. This feedback corrects the vertical displacement of the gripping portion 307 at the tip of the manipulator due to gravity.

第６図は本発明による位置決めの補償装置の他の実施例
を示すロボットの制御アルゴリズムのブロック線図であ
る。第６図において、ファジー関数によるファジー制御
を用いて目標値を補正する位置決めの補償装置の例を示
し、６０１はファジールール、６０２〜６０７はファジ
ールール６０１により求めた姿勢・位置の目標値１０１
〜１１０への補正値である。第６図の第１図との違いは
目標値発生回路１０１と逆変換１０２の間の姿勢・位置
の目標値１０５〜１１０の補正を第１図の現在のマニピ
ュレータの上下方向位置１２９を用いて補償論理１０３
で上下方向の位置の目標値１１０を補正することにより
行うのではなく、姿勢・位置の目標値１０５〜１１０の
補正をファジールール６０１で求めた姿勢・位置の目標
値１０５〜１１０への補正値６０２〜６０７により行っ
ている点である。なおこの制御系のハードウェアのシス
テム構成は第１図の実施例と同じく第２図および第３図
に示すロボットのシステムである。FIG. 6 is a block diagram of a robot control algorithm showing another embodiment of the positioning compensation device according to the present invention. FIG. 6 shows an example of a positioning compensation device that corrects a target value using fuzzy control using a fuzzy function, 601 is a fuzzy rule, and 602 to 607 are posture/position target values 101 determined by the fuzzy rule 601.
It is a correction value to ~110. The difference between FIG. 6 and FIG. 1 is that the target values 105 to 110 of the posture/position between the target value generation circuit 101 and the inverse conversion 102 are corrected using the current vertical position 129 of the manipulator in FIG. Compensation logic 103
Instead of correcting the target value 110 of the position in the vertical direction with This is done by steps 602-607. The hardware system configuration of this control system is the same as that of the embodiment shown in FIG. 1, and is the robot system shown in FIGS. 2 and 3.

つぎにこの構成のファジールール６０１について説明す
る。このファジーモデルとして次の３つのプラント法則
をとるものとする。Next, the fuzzy rule 601 having this configuration will be explained. Assume that the following three Plant's laws are adopted as this fuzzy model.

ｖ：ｓｆｎ工＋Ｙ’　ｉｓ　Ｂｉｇ　ｔｈｅｎ　Ｙ’Ｈ
＝　ｆ　：Ｌ’　：　ｉｆ　５ア「イｉｓ　Ｓｍａｌｌ
　ａｎｄ　Ｚ　ｉｓ　Ｂｉｇｔｈｅｎ　Ｙｊ＝　　ｆ　
７ Ｌ：ｌ：　ｉｆ　％　ｉｓ　Ｓｍａｌｌ　ａｎｄ　Ｚ　
ｉｓ　Ｓｍａｌｌｔｈｅｎ　ｙ：＝　　ｆ３．　　　（
ｉ＝１−６）ここでＸはマニピュレータ先端の把持部３
０７の前後方向位置の目標値１０８．　Ｙは左右方向位
置の目標値１０９．２は上下方向位置の目標値１１０で
ある。v:sfn 工＋Y' is Big then Y'H
= f : L' : if 5A "Is Small
and Z is Bigthen Yj= f
7 L:l: if % is Small and Z
is Small then y:= f3. (
i=1-6) Here, X is the gripping part 3 at the tip of the manipulator
Target value of longitudinal position of 07 108. Y has a target value of 109 for the horizontal position.2 is a target value of 110 for the vertical position.

また後件部関数ｆ：ｌｆｔ＋ｆ？は経験則による関数で
あってｉ＝１でマニピュレータ先端の把持部３０７の姿
勢ロール角目標値１０５に対する補正値６０２、ｉ＝２
でピッチ角目標値１０６に対する補正値６０３、ｉ＝３
でヨー角目標値１０７に対する補正値６０４、ｉ＝４で
前後方向位置目標値１０８に対する補正値６０５、ｉ＝
５で左右方向位置目標値１０９に対する補正値６０６、
ｉ＝６で上下方向位置目標値１１０に対する補正値１０
７を与えるものとする。つぎに５ア「いにおけるファジ
ー集合ｒＢｉｇＪのメンバシップ関数をＡ、、Ｊｘ２＋
ｙ２におけるファジー集合ｒ　Ｓ　ｍａｌｌ　Ｊのメン
バシップ関数をＡｔ、Ｚにおけるファジー集合ｒＢｉｇ
Ｊのメンバシップ関数をＡ３、Ｚにおけるファジー集合
ｒｓｍａｌｌＪのメンバシップ関数をＡ４とすると、各
ｉ＝１〜６の目標値１０５〜１１０に対する補正値６０
２〜６０７の補正値ｍＨ（ｉ＝１〜６）は次式で与えら
れる。Also, the consequent function f:lft+f? is a function based on an empirical rule, and when i=1, the correction value 602 for the posture roll angle target value 105 of the grip part 307 at the tip of the manipulator, i=2
and the correction value 603 for the pitch angle target value 106, i=3
, the correction value 604 for the yaw angle target value 107, the correction value 605 for the longitudinal direction position target value 108 for i=4, i=
5, a correction value 606 for the horizontal position target value 109,
When i=6, the correction value is 10 for the vertical position target value 110.
7 shall be given. Next, let us define the membership function of the fuzzy set rBigJ in 5A as A, , Jx2+
The membership function of the fuzzy set r S mall J in y2 is At, the fuzzy set rBig in Z
Assuming that the membership function of J is A3 and the membership function of the fuzzy set rsmallJ in Z is A4, the correction value 60 for each target value of 105 to 110 for each i=1 to 6.
The correction value mH of 2 to 607 (i=1 to 6) is given by the following equation.

第７図は本発明による位置決めの補償装置のさらに他の
実施例を示すロボットの制御アルゴリズムのブロック線
図である。第７図において、画像情報を用いて目標値を
補正する位置決めの補償装置の例を示し、７０１は画像
情報解析論理、７０２は画像情報解析論理７０１からの
上下方向位置７０２，７０３は画像信号である。また第
８図は第７図のロボットのシステム構成図である。第８
図において、８０１は画像入力装置、８０２は画像処理
装置である。第８図の第２図との違いは画像入力装置８
０１と画像処理装置８０２を追加した点で、マニピュレ
ータ先端の把持部３０７の部分の様子は画像入力装置８
０１で入力し、画像処理装置８０２で画像処理したのち
、中央演算装置２０１ヘマニピユレータ先端の把持部３
０７の上下方向位置７０３の信号として取り込む。この
ときの第７図の中央演算装置２０１内部での制御アルゴ
リズムのブロック線図の第１図との違いは、第１図では
位置決めの補償論理１０３へのフィードバック信号とし
て第１〜第６関節の現在の関節角１２３〜１２８から正
変換１０４で正変換して得た現在のマニピュレータ先端
の把持部３０７の上下方向位置１２９を用いているのに
対して、第７図では第１図の上下方向位置１２９の代り
に該上下方向位置１２９に対応するものとして、画像処
理装置８０２からの画像信号７０３を画像情報解析論理
７０１で解析して得られる現在のマニピュレータ先端の
把持部３０７の上下方向位置７０２の信号を補償論理１
０３へのフィードバック信号としていることである。こ
の実施例によれば、実際のマニピュレータ先端の把持部
３０７の上下方向の現在位置を補償論理１０３ヘフイー
ドバツクしているので、マニピユレータ部のアームのた
わみ等にも対応できる。FIG. 7 is a block diagram of a robot control algorithm showing still another embodiment of the positioning compensation device according to the present invention. FIG. 7 shows an example of a positioning compensation device that corrects a target value using image information, 701 is an image information analysis logic, 702 is an image information analysis logic, and vertical positions 702 and 703 from the image information analysis logic 701 are image signals. be. FIG. 8 is a system configuration diagram of the robot shown in FIG. 7. 8th
In the figure, 801 is an image input device, and 802 is an image processing device. The difference between FIG. 8 and FIG. 2 is that the image input device 8
01 and the image processing device 802, the appearance of the grip portion 307 at the tip of the manipulator is similar to that of the image input device 8.
01, and after image processing by the image processing device 802, the central processing unit 201 handles the grip part 3 at the tip of the hemanipulator.
07 as a signal at the vertical position 703. At this time, the difference between the block diagram of the control algorithm inside the central processing unit 201 in FIG. 7 and FIG. 1 is that in FIG. The current vertical position 129 of the gripping part 307 at the tip of the manipulator obtained by performing positive transformation 104 from the current joint angles 123 to 128 is used, whereas in FIG. Instead of the position 129, the current vertical position 702 of the gripping part 307 at the tip of the manipulator is obtained by analyzing the image signal 703 from the image processing device 802 using the image information analysis logic 701, as corresponding to the vertical position 129. compensation logic 1
03 as a feedback signal. According to this embodiment, since the current position in the vertical direction of the gripping section 307 at the tip of the actual manipulator is fed back to the compensation logic 103, it is possible to cope with deflection of the arm of the manipulator section, etc.

以上の実施例によれば、位置決め装置の制御系における
位置決めの補償手段で補正を行うことにより、目標値を
操作してマニピュレータ先端の把持部の重力等による変
位を補正できる。なお上記の実施例は重力の作用するロ
ボットシステムの位置決め制御系について説明したが、
本発明は一定方向の力が位置決め装置および位置決めさ
れる対象物の少なくとも一方に作用する場合の位置決め
制御系に同様に適用可能である。According to the embodiments described above, by performing correction using the positioning compensation means in the control system of the positioning device, it is possible to correct the displacement of the gripping portion at the tip of the manipulator due to gravity or the like by manipulating the target value. Note that the above embodiment describes a positioning control system for a robot system where gravity acts.
The present invention is similarly applicable to a positioning control system in which a force in a fixed direction acts on at least one of a positioning device and an object to be positioned.

［発明の効果］ 本発明によれば、位置決め装置および位置決めされる対
象物の少なくとも一方に一定方向のカが作用する位置決
め制御系において、大量な演算を施さなくても簡易に一
定方向のカによる対象物の変位量を補正できるので、制
御系の設計が簡単になるうえ計算機の負担軽減に基づく
応答性の向上がもたらされ、精度よい位置決め制御を行
えるという効果がある。[Effects of the Invention] According to the present invention, in a positioning control system in which a force in a fixed direction acts on at least one of a positioning device and an object to be positioned, it is possible to easily control the force in a fixed direction without performing a large amount of calculations. Since the amount of displacement of the object can be corrected, the design of the control system is simplified, and the responsiveness is improved by reducing the load on the computer, which has the effect of allowing highly accurate positioning control.

【図面の簡単な説明】[Brief explanation of the drawing]

第１図は本発明による位置決めの補償装置の一実施例を
示すブロック線図、第２図は第１図のシステム構成図、
第３図はマニピュレータ外観図、第４図は第１図の１関
節のみのモデル構成図、第５図は第４図のブロック線図
、第６図は本発明による他の実施例を示すブロック線図
、第７図は本発明によるさらに他の実施例を示すブロッ
ク線図、第８図は第７図のシステム構成図である。 １０１・・・目標値発生論理、１０２・・・ラプラス逆
変換、１０３・・位置決めの補償論理、１０４・・・ラ
プラス正変換、１０５〜１１０・・姿勢・位置の目標値
、１１１〜１１６・・・関節角目標値、１１７〜１２２
・・・関節指令値、１２３〜１２８・・・現在の関節角
、１２９・・・現在の上下方向位置、２０１・・・中央
演算装置、２０２・・・Ｄ／Ａ変換器、２０３・・・パ
ワーアンプ、２０４〜２０９・・・関節駆動装置、２１
０・・・把持部駆動装置、２１１〜２１６・・・関節角
検出器、２１７・・・ディジタル入力器、３０１〜３０
６・・・関節、３０７・・・把持部、５０８・・・位置
決め補償論理、６０１・・・ファジールール、７０１・
・・画像情報解析論理、７０２・・・現在の上下方向位
置、７０３・・・画像信号、８０１・・・画像入力装置
、８０２・・・画像処理装置。 代理人　弁理士　　秋　本　正　実 第 ＼、　　　　　　　　　□又２１Ｑ 図 ３０１〜３０６第１〜第６関節 ３０７ｚ乃仰 第６図FIG. 1 is a block diagram showing an embodiment of the positioning compensation device according to the present invention, FIG. 2 is a system configuration diagram of FIG. 1,
Fig. 3 is an external view of the manipulator, Fig. 4 is a model configuration diagram of only one joint in Fig. 1, Fig. 5 is a block diagram of Fig. 4, and Fig. 6 is a block diagram showing another embodiment according to the present invention. FIG. 7 is a block diagram showing still another embodiment of the present invention, and FIG. 8 is a system configuration diagram of FIG. 7. 101... Target value generation logic, 102... Laplace inverse transformation, 103... Positioning compensation logic, 104... Laplace forward transformation, 105-110... Target value of posture/position, 111-116...・Joint angle target value, 117-122
...Joint command value, 123-128...Current joint angle, 129...Current vertical position, 201...Central processing unit, 202...D/A converter, 203... Power amplifier, 204-209...Joint drive device, 21
0...Gripper driving device, 211-216...Joint angle detector, 217...Digital input device, 301-30
6... Joint, 307... Grasping part, 508... Positioning compensation logic, 601... Fuzzy rule, 701...
. . . Image information analysis logic, 702 . . . Current vertical position, 703 . . . Image signal, 801 . . . Image input device, 802 . Agent Patent Attorney Tadashi Akimoto Minoru \, □Mata 21Q Figures 301-306 1st to 6th joints 307z No.6 Figure 6

Claims (1)

【特許請求の範囲】 １、位置決め装置および位置決めされる対象物の少なく
とも一方に一定方向の力が作用している場合の位置決め
制御系において、真の目標値に対して上記一定方向の力
の向きと逆方向にオフセットを加えたものを目標値とし
て与える手段を備えたことを特徴とする位置決めの補償
装置。 ２、上記オフセットの値をファジー関数で与えることを
特徴とする請求項１記載の位置決めの補償装置。 ３、上記オフセットの値を決定するための対象物の位置
情報を画像を利用することによって得ることを特徴とす
る請求項１記載の位置決めの補償装置。 ４、上記一定方向の力が重力である場合の請求項１記載
の位置決めの補償装置。[Claims] 1. In a positioning control system where a force in a fixed direction is acting on at least one of the positioning device and the object to be positioned, the direction of the force in the fixed direction with respect to the true target value. A positioning compensation device characterized by comprising means for giving a target value obtained by adding an offset in the opposite direction to the above. 2. The positioning compensation device according to claim 1, wherein the offset value is given by a fuzzy function. 3. The positioning compensation apparatus according to claim 1, wherein the positional information of the object for determining the offset value is obtained by using an image. 4. The positioning compensation device according to claim 1, wherein the force in a certain direction is gravity.