JPH11114859A - Control method for force and position of manipulator and detecting method for force used therefor and control device for force and position - Google Patents

Abstract

PROBLEM TO BE SOLVED: To detect and control force without a sensor by finding model of a manupulator whose hand satisfies a restricting condition, calculating force of the hand to a plane from an equation of motion introduced from the model, and finding and controlling input necessary to make the calculated force coincide with a desired value. SOLUTION: Modeling of a robot to perform grinding work by a hand 2 on the tip of a manupulator 1 while contacting with a restricting surface 3, is performed. A system is described by an equation of motion on the basis of its model, and a speed of generalized coordinates and a condition to satisfy acceleration, are found under a restricting condition to be satisfied, and this is solved on restricting force (f). Since this becomes an inverse dynamics calculation, the restricting force (f) can be found by an operation without using a force sensor. Therefore, force control can be performed without using the force sensor.

Description

【発明の詳細な説明】DETAILED DESCRIPTION OF THE INVENTION

【０００１】[0001]

【発明の属する技術分野】本発明は、機器の組立、グラ
インディング、バリ取り等の作業を行うロボット、特に
そのマニピュレータの力及び位置の制御方法及びこれに
用いる力の検出方法、並びに力及び位置の制御装置に関
する。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a robot for performing operations such as assembling, grinding, and deburring equipment, and more particularly to a method for controlling the force and position of a manipulator thereof, a method for detecting a force used for the same, and a method for detecting force and position. Related to a control device.

【０００２】[0002]

【従来の技術】グラインディング作業等の場合マニピュ
レータの手先は、作業対象物の面に所定の力で押し付け
る制御と、同時に、面の接線方向に対しては所定の速度
又は加速度で移動させる制御を行うことが必要とされ
る。このような力と位置に対する制御方法として力と位
置を夫々直接的に制御する方法と、マニピュレータに作
用する力とその運動との関係に望ましい特性を持たせる
ことで間接的に作用力を制御する方法とがある。前者の
例としてはハイブリッド制御方法、動作ハイブリッド制
御方法、適応制御方法、線形制御方法、非線形フィード
バック制御方法等がある。また後者の例としては剛性制
御方法、ダンピング制御方法、インピーダンス制御方法
等がある。2. Description of the Related Art In the case of a grinding operation or the like, the hand of a manipulator is controlled so as to be pressed against a surface of a work object with a predetermined force, and at the same time, is controlled to move at a predetermined speed or acceleration in a tangential direction of the surface. It is required to do. As a control method for such a force and a position, a method of directly controlling the force and the position, and a method of controlling the acting force indirectly by giving a desirable characteristic to the relationship between the force acting on the manipulator and the movement thereof. There is a way. Examples of the former include a hybrid control method, an operation hybrid control method, an adaptive control method, a linear control method, and a non-linear feedback control method. Examples of the latter include a stiffness control method, a damping control method, and an impedance control method.

【０００３】従来マニピュレータの手先を作業対象物の
面に対し所定の力で押し付けるための制御は、手先に被
加工物に対する押し付け力を測定するための力センサ、
例えば歪みゲージを設けておき、この力センサの検出値
を予め定めた目標値と比較し、目標値に一致するようマ
ニピュレータの関節に作用させるトルクを調節すること
で行われている。Conventionally, control for pressing a hand of a manipulator against a surface of a work object with a predetermined force is performed by a force sensor for measuring a pressing force on a hand against a workpiece.
For example, a strain gauge is provided, the detection value of the force sensor is compared with a predetermined target value, and the torque applied to the joint of the manipulator is adjusted to match the target value.

【０００４】[0004]

【発明が解決しようとする課題】ところでこのような従
来方法にあっては、力センサは、手先が面に対して作用
させるあらゆる方向の力の検出を可能とするために構造
が複雑となり、コストアップを免れ得ないのが現状であ
る。By the way, in such a conventional method, the force sensor has a complicated structure because it can detect a force in all directions which the hand can act on the surface, and the cost is low. It is the present situation that we cannot escape up.

【０００５】本発明はかかる事情に鑑みてなされたもの
であって、その目的とするところは前述した拘束力の決
定方法をロボットにおけるマニピュレータのハンドの拘
束運動に適用し、拘束条件を常に満足するマニピュレー
タの運動方程式を導出し、これに基づきマニピュレータ
の力及び位置の制御方法及びこれに用いる力の検出方法
並びに力及び位置の制御装置を提供するにある。The present invention has been made in view of such circumstances, and a purpose thereof is to apply the above-described method for determining a restraining force to a restraining motion of a hand of a manipulator in a robot and always satisfy a restraining condition. An object of the present invention is to provide a method of controlling a force and a position of a manipulator, a method of detecting a force used therein, and a control device of a force and a position based on the equation of motion of the manipulator.

【０００６】[0006]

【課題を解決するための手段】第１の発明に係るマニピ
ュレータの力制御方法は、対象物の面との間に、これに
対して力を作用させつつ面の接線方向に移動させるとい
う拘束条件が与えられたハンドを持つマニピュレータの
前記力の制御方法において、ハンドが前記拘束条件を満
たすマニピュレータのモデルを求め、該モデルから導い
た運動方程式から前記面に対するハンドの力を算出し、
算出した力を目標値に一致させるに必要なマニピュレー
タに対する入力を求め、該入力に基づき前記面に対する
ハンドの力を制御することを特徴とする。According to a first aspect of the present invention, there is provided a method for controlling a force of a manipulator, wherein a force is applied between the object and a surface of the object while the surface is moved in a tangential direction of the surface. In the method of controlling the force of a manipulator having a given hand, the hand determines a manipulator model that satisfies the constraint condition, and calculates the force of the hand on the surface from the equation of motion derived from the model,
An input to the manipulator required to match the calculated force to the target value is obtained, and the force of the hand on the surface is controlled based on the input.

【０００７】第２の発明に係るマニピュレータの力検出
方法は、対象物の面との間に、これに対して力を作用さ
せつつ面の接線方向に移動させるという拘束条件が与え
られたハンドを持つマニピュレータの前記力を検出する
方法において、ハンドが前記拘束条件を満たすマニピュ
レータのモデルを求め、該モデルから導かれた運動方程
式より前記面に対するハンドの力を算出することを特徴
とする。According to a second aspect of the present invention, there is provided a method for detecting a force of a manipulator, wherein a hand provided with a constraint between a surface of an object and a tangential direction of the surface while applying a force to the surface is applied. In the method of detecting the force of a manipulator having the method, a hand obtains a manipulator model that satisfies the constraint condition, and calculates the force of the hand on the surface from a motion equation derived from the model.

【０００８】第３の発明に係るマニピュレータの力検出
方法は、前記ハンドの力ｆは下式に従って算出すること
を特徴とする。In a third aspect of the present invention, in the method for detecting a force of a manipulator, the force f of the hand is calculated according to the following equation.

【０００９】[0009]

【数２】 (Equation 2)

【００１０】第４の発明に係るマニピュレータの力及び
位置制御方法は、対象物の面との間に、これに対して力
を作用させつつ面の接線方向に移動させるという拘束条
件が与えられたハンドを持つマニピュレータの前記力及
び位置の制御方法において、ハンドが前記拘束条件を満
たすマニピュレータのモデルを求め、該モデルから導い
た運動方程式から前記ハンドの力を演算し、演算したハ
ンドの力が目標値に一致し、またハンドの位置が目標軌
道を満足するに必要なマニピュレータに対する入力を演
算し、該入力に基づいてマニピュレータを制御すること
を特徴とする。In the force and position control method for a manipulator according to a fourth aspect of the present invention, a constraint condition that a force is applied to the object and the object is moved in a tangential direction of the surface is provided. In the method for controlling the force and position of a manipulator having a hand, the hand obtains a manipulator model that satisfies the constraint condition, calculates the hand force from a motion equation derived from the model, and calculates the calculated hand force as a target. An input to the manipulator that matches the value and that is necessary for the position of the hand to satisfy the target trajectory is calculated, and the manipulator is controlled based on the input.

【００１１】第５の発明に係るマニピュレータの力及び
位置制御装置は、対象物の面との間に、これに対して力
を作用させつつ面の接線方向に移動させるという拘束条
件が与えられたハンドを持つマニピュレータの力及び位
置を制御する装置において、対象物に対するハンドの力
を演算する手段と、演算したハンドの力とハンドの目標
拘束力との偏差を解消すべく、マニピュレータに対する
第１の入力を演算する手段と、対象物に対するハンドの
位置と、対象物に対するハンドの目標軌道との偏差を解
消すべくマニピュレータに対する第２の入力を演算する
手段と、前記マニピュレータに対する第１、第２の入力
の和及び予め求めたマニピュレータのモデル式とに基づ
いてハンドの設定位置を出力する手段とを備えることを
特徴とする。In the force and position control device for a manipulator according to the fifth aspect of the present invention, a constraint condition is provided that the force and the force are applied to the manipulator and the object is moved in a tangential direction of the surface. In a device for controlling a force and a position of a manipulator having a hand, means for calculating a force of the hand on an object, and a first device for the manipulator for eliminating a deviation between the calculated force of the hand and a target constraint force of the hand. Means for calculating an input; means for calculating a second input to the manipulator to eliminate a deviation between the position of the hand with respect to the object and a target trajectory of the hand with respect to the object; first and second means for the manipulator Means for outputting the set position of the hand based on the sum of the inputs and the manipulator model formula obtained in advance.

【００１２】[0012]

【発明の実施の形態】以下本発明をその実施の形態を示
す図面に基づき具体的に説明する。図１はロボットによ
るグラインディング作業の状態を示す模式図であり、図
中１は剛体リンクからなる運動学的冗長性（ハンドの位
置を実現するマニピュレータ形状の任意性がある状態）
のないロボットのマニピュレータ、２はその手先効果
器、３は被加工面を示している。先ず、拘束面に接触し
つつマニピュレータ先端のハンドによりグラインディン
グ作業を行うロボットのモデリングを行う。ただし接触
点での摩擦力は無視できるものとする。ラグラジアンを
Ｌ、一般化座標を＊ｑ∈Ｒ^l （ベクトルの符号ｑは＊ｑ
と表示する、以下同じ。ただし、式中では＊を省略し、
また行列にも＊をつけてスカラーと区別することとす
る。）、一般化入力を＊τ∈Ｒ^l 、＊ｓ∈Ｒ^m をラグラ
ンジュの未定常数ベクトルとすると、ハンドが拘束状態
にあるマニピュレータのラグランジュの方程式は下記
(1) 式で表わせる。DESCRIPTION OF THE PREFERRED EMBODIMENTS Hereinafter, the present invention will be described in detail with reference to the drawings showing the embodiments. FIG. 1 is a schematic view showing a state of a grinding operation by a robot. In the figure, 1 is a kinematic redundancy composed of rigid links (a state in which a manipulator shape for realizing a hand position has an arbitrary shape).
The manipulator of the robot without the mark, 2 is its hand effector, and 3 is the surface to be machined. First, modeling of a robot that performs a grinding operation by a hand at the tip of the manipulator while contacting the constraint surface is performed. However, the frictional force at the contact point shall be negligible. Lagradian is L, generalized coordinates are * q∈R ^l (the sign q of the vector is * q
And the same below. However, * is omitted in the formula,
The matrix is also marked with * to distinguish it from a scalar. ), If the generalized inputs are * τ∈R ^l and * s∈R ^m are Lagrange's unsteady number vector, the Lagrangian equation of the manipulator with the hand in the constrained state is
It can be expressed by equation (1).

【００１３】[0013]

【数３】 (Equation 3)

【００１４】一方手先の位置・姿勢ベクトル＊ｒ∈
Ｒ^l 、被加工物の面、即ち拘束条件＊Ｃ∈Ｒ^m は順運動
学関係式から下記(2) 、(3) 式で表わせる。 ｒ＝ｒ（ｑ） …(2) Ｃ｛ｒ（ｑ）｝＝０ …(3) ここで拘束されるハンドは作業座標系の中で非拘束方向
に運動できる自由度が残されていることからｌ＞ｍであ
る。ハンドに作用する拘束力を＊ｆとすると＊ｓと＊ｆ
との関係は下記(4) 式で表わせる。One hand's position / posture vector * r ベ ク ト ル
R ^l and the surface of the workpiece, that is, the constraint condition * C∈R ^m , can be expressed by the following equations (2) and (3) from the forward kinematics relational equation. r = r (q)... (2) C {r (q)} = 0... (3) The hand to be constrained has a degree of freedom to move in the non-constrained direction in the work coordinate system. And l> m. Assuming that the binding force acting on the hand is * f, * s and * f
Can be expressed by the following equation (4).

【００１５】[0015]

【数４】 (Equation 4)

【００１６】但し、‖・‖は＊・のユークリッドノルム
を表す。拘束力＊ｆが作用している状態のマニピュレー
タモデルは(1) 式と(4) 式より下記(5) 式となる。However, 但 · ‖ represents the Euclidean norm of * ·. The manipulator model in a state where the constraint force * f is acting is given by the following equation (5) from the equations (1) and (4).

【００１７】[0017]

【数５】 (Equation 5)

【００１８】(5) 式より、運動方程式である(6) 式が導
かれる。From equation (5), equation (6), which is the equation of motion, is derived.

【００１９】[0019]

【数６】 (Equation 6)

【００２０】(7) 式の如き運動方程式で記述されたシス
テムは(3) 式で表される拘束条件を常に満たさなければ
ならない。そこで(3) 式を時間ｔで２回微分し、一般化
座標の速度、加速度が満たさなければならない条件を求
めると、(8) 式となる。The system described by the equation of motion such as equation (7) must always satisfy the constraint condition expressed by equation (3). Then, the equation (3) is differentiated twice with respect to the time t, and the condition that the velocity and acceleration of the generalized coordinates must satisfy is obtained as the equation (8).

【００２１】[0021]

【数７】 (Equation 7)

【００２２】＊Ｃ∈Ｒ^m が独立な拘束条件であることか
ら、∂Ｃ／∂ｘ₁ ∈Ｒ^{m ・ l} は行フルランクであるこ
と、＊Ｍ^-1は正則な行列であることから、＊ｆの係数行
列（∂Ｃ／∂ｘ₁ ）Ｍ^-1（∂Ｃ／∂ｘ₁ ）^T ∈Ｒ^{m ・ m}
は＊ｘ₁ に拘らず正則な行列であり、(10)式は＊ｆに関
して一意に解くことができる。＊ｆの解は(11)式で与え
られる。Since * C∈R ^m is an independent constraint, ∂C / ∂x ₁ ∈R ^{m · l} is a row full rank, and * M ^-1 is a regular matrix. * f the coefficient matrix ^{_{(∂C / ∂x 1) M -1}} (∂C / ∂x 1) T ∈R m · m
Is * a Notwithstanding nonsingular matrix x _1, (10) equation can be solved uniquely with respect to * f. The solution of * f is given by equation (11).

【００２３】[0023]

【数８】 (Equation 8)

【００２４】[0024]

【数９】 (Equation 9)

【００２５】従って初期条件ｘ₁ (0) とTherefore, the initial conditions x ₁ (0) and

【００２６】[0026]

【数１０】 (Equation 10)

【００２７】が(3) 式及びその時間微分を満足するとき
(3) 式は時間に拘らず成り立ち、(14)式を(7) 式に代入
して得られる状態方程式は拘束運動を表現した(15)式の
如き運動方程式となる。When satisfies equation (3) and its time derivative
Equation (3) holds regardless of time, and the equation of state obtained by substituting equation (14) into equation (7) is a motion equation such as equation (15) expressing the constrained motion.

【００２８】[0028]

【数１１】 [Equation 11]

【００２９】なお、ここでは運動学的冗長性を持たない
マニピュレータを仮定しているのでハンド位置＊ｒ、一
般化座標＊ｑ及び一般化入力＊τの次元は等しく、ｌで
ある。(14)式より、＊ｆを実現する＊τはｌ＞１より冗
長である。ここではこれを力制御冗長性と称す。(14)式
から拘束力＊ｆの目標値＊ｆ_d を実現する一般化入力＊
τは(16)式で与えられる。Since a manipulator having no kinematic redundancy is assumed here, the dimensions of the hand position * r, the generalized coordinates * q and the generalized input * τ are equal to one. From equation (14), * τ that realizes * f is more redundant than l> 1. This is referred to herein as force control redundancy. (14) generalize to achieve the target value * f _d of binding * f from type input *
τ is given by equation (16).

【００３０】[0030]

【数１２】 (Equation 12)

【００３１】(16)式に含まれる＊ａ（＊ｘ），＊Ａ（＊
ｘ₁ ）は＊Ｍ（＊ｑ）、＊ｈ（＊ｘ）及び＊ｑ（＊
ｘ₁ ）を含むが、これらはマニピュレータの動力学パラ
メータと＊ｘにより定まる行列及びベクトルである。(1
6)式及び後述の(18)式より＊ｆ_d＝＊ｆが達成されるこ
とが(16)式の＊τを(14)式に代入することで確かめられ
る。なお、動力学パラメータの真値が得られない場合に
は、* A (* x), * A (*) included in equation (16)
x ₁ ) is * M (* q), * h (* x) and * q (*
x ₁ ), which are matrices and vectors defined by manipulator dynamics parameters and * x. (1
It can be confirmed that * f _d = * f is achieved by substituting * τ in equation (16) into equation (14) from equation (6) and equation (18) described later. If the true value of the kinetic parameter cannot be obtained,

【００３２】[0032]

【数１３】 (Equation 13)

【００３３】とすると(16)式は次式となり、Then, equation (16) becomes the following equation.

【００３４】[0034]

【数１４】 [Equation 14]

【００３５】(17)式を用いて制御を行うこともできる。
(17)式中の任意ベクトル＊ｋを自由運動可能な方向に対
する位置制御に用いることとし、速度ダンピング項を入
れて(18)式で与える。The control can be performed by using the equation (17).
The arbitrary vector * k in the equation (17) is used for position control in the direction in which free movement is possible, and the equation is given by the equation (18) including a velocity damping term.

【００３６】[0036]

【数１５】 (Equation 15)

【００３７】図２はコントローラの制御内容を示すブロ
ック線図である。ハンドの目標軌道＊ｒ_d 及び拘束力の
目標値＊ｆ_d が入力されると、夫々についての位置制御
ループと力制御ループとによる制御が略同時的に進行す
る。先ず目標軌道＊ｒ_d が入力されると、この目標軌道
＊ｒ_d と現在のハンドの検出軌道であるフィードバック
値との偏差が求められ、これに位置制御ゲイン行列＊Ｋ
_P を乗じた値に〔∂ｒ／∂ｑ〕^T を乗じ、この値から関
節角速度＊ｘ₂ にダンピング行列Ｋ_V を乗じた値を減算
し、任意ベクトル＊ｋを(18)式に従って算出する。任意
ベクトル＊ｋに (Ｉ−Ａ⁺ Ａ）を乗じた値と拘束力の目
標値＊ｆ_d とFIG. 2 is a block diagram showing the control contents of the controller. When target value * f _d of the target trajectory * r _d and binding of the hand is input, the control by the position control loop and a force control loop for each proceeds substantially simultaneously. First the target trajectory * r _d is inputted, the target trajectory * r _d a deviation between the feedback value is the detection orbit of the current hand is determined, this position control gain matrix * K
The value obtained by multiplying the _P multiplied by [∂r / ∂q] ^T, a value obtained by multiplying the damping matrix K _V is subtracted from this value to joint angular velocity * x _2, calculated according to any vector * k (18) formula . The value obtained by multiplying the arbitrary vector * k by (IA ⁺ A) and the target value * f _{d of the} constraint force are

【００３８】[0038]

【数１６】 (Equation 16)

【００３９】との偏差に、＊Ａ⁺ （ｘ₁ ）、即ち＊Ａ
（＊ｘ₁ ）の疑似逆行列を乗じた値を差し引く(16)式に
従って一般化入力＊τを得る。この一般化入力＊τを入
力されたハンド拘束状態の実際のマニピュレータは(15)
式に示す運動方程式に従って運動を発生する。その運動
結果である* A ⁺ (x ₁ ), that is, * A
The generalized input * τ is obtained according to the equation (16) in which the value obtained by multiplying the pseudo inverse matrix of (* x ₁ ) is subtracted. The actual manipulator in the hand restrained state to which this generalized input * τ is input is (15)
Motion is generated according to the equation of motion shown in the equation. The result of that exercise

【００４０】[0040]

【数１７】 [Equation 17]

【００４１】は、ロータリーエンコーダ等で容易に検出
でき、一般化座標値＊ｑを得、手先の位置・姿勢ベクト
ル＊ｒ（＊ｑ）から手先の位置・姿勢＊ｒを得る。Can be easily detected by a rotary encoder or the like, a generalized coordinate value * q is obtained, and a hand position / posture * r is obtained from a hand position / posture vector * r (* q).

【００４２】この制御系は真値＊ａ（＊ｘ）、＊Ａ（＊
ｘ₁ ）が求まる場合、(17)式を(14)式に代入することで
(15)式で表わされる運動が如何なる状態かに拘らず常に
＊ｆ＝＊ｆ_d を達成出来る。これは(14)式で表わされる
ように入力τによって発生するｆは、微分方程式で表わ
されているのではなく、代数方程式であるから、時間遅
れなく発生するということから裏付けられる。This control system has true values * a (* x), * A (*
If x ₁₎ is obtained, by substituting the equation (17) to (14)
(15) always movement of formula regardless of whether any state * f = * f _d to be achieved. This is supported by the fact that the f generated by the input τ as expressed by the equation (14) is not expressed by a differential equation but is an algebraic equation, and therefore occurs without a time delay.

【００４３】次に本願発明方法について図３に示す如く
シミュレーションモデルを用いて行った位置・力制御実
験について説明する。シミュレーションモデルにおいて
は一次元の剛体として取扱った２リンクのマニピュレー
タを用いており、リンク４、５の各一端周辺及びリンク
４の基端と基台とはジョイント６、７によって枢支連結
され、またリンク５の先端にはグラインダが取り付けら
れている。このグラインダを壁面３に対して作用力Ｆ_y
＝Ｆ_y（ｔ）で押し付けつつこの壁面３に沿って速度Ｖ
_x ＝Ｖ_x （ｔ）で移動させつつ研削を行ってゆくものと
した。なおリンクの寸法仕様、入力トルク等の数値は表
１に示すとおりである。Next, a description will be given of a position / force control experiment conducted on the method of the present invention using a simulation model as shown in FIG. In the simulation model, a two-link manipulator treated as a one-dimensional rigid body is used. The periphery of each end of the links 4 and 5 and the base end of the link 4 and the base are pivotally connected by joints 6 and 7, and A grinder is attached to the tip of the link 5. This grinder is applied to the wall 3 with an acting force F _y
= F _y (t) and velocity V along this wall 3
Grinding was performed while moving at _x = V _x (t). The numerical values of the link dimensions, input torque, etc. are as shown in Table 1.

【００４４】[0044]

【表１】 [Table 1]

【００４５】[0045]

【数１８】 (Equation 18)

【００４６】また、作業表面をｘ軸に平行な直線とし、
拘束面をｙ＝ｙ_d で表すと拘束条件Ｃは、 Ｃ（＊ｒ（＊ｑ））＝ｙ_d −ｙ＝ｙ_d −ｌ₁ ｓ₁ −ｌ₂
ｓ₁₂＝０ となる。ここでは、拘束面はｙ_d ＝０．５〔ｍ〕とし
た。ｙ軸方向に拘束されているため、作用力目標値はｆ
_d ＝ｆ_ydとなり、ハンドはｘ軸方向のみ運動可能とな
る。The work surface is a straight line parallel to the x-axis,
Constraint C represent the restraining surface at y = y _d is, C (* r (* q )) = y d -y = y d -l 1 s 1 -l 2
s ₁₂ = 0. Here, the constraining surface was set to y _d = 0.5 [m]. Since it is constrained in the y-axis direction, the target value of the acting force is f
_d = f _yd , and the hand can move only in the x-axis direction.

【００４７】[0047]

【数１９】 [Equation 19]

【００４８】が満たさなければならない条件式を与える
が、シミュレーションモデルの拘束条件を作業座標系で
表すとA conditional expression that must be satisfied is given. When the constraint condition of the simulation model is expressed in the working coordinate system,

【００４９】[0049]

【数２０】 (Equation 20)

【００５０】となる。(15)式の運動方程式の解は、常にIs as follows. The solution of equation (15) is always

【００５１】[0051]

【数２１】 (Equation 21)

【００５２】ハンドの初期位置及び速度をThe initial position and speed of the hand

【００５３】[0053]

【数２２】 (Equation 22)

【００５４】と与え、自由運動可能なｘ軸方向の## EQU10 ## and the x-axis direction in which free movement is possible

【００５５】[0055]

【数２３】 (Equation 23)

【００５６】と設定する。マニピュレータの初期形状は
図３に示す状態である。(18)式中のコントローラのゲイ
ンを ＊Ｋ_P ＝diag（１７００，１７００）〔Ｎ／ｍ〕， ＊Ｋ_V ＝diag（３０，３０）〔Ｎｓ／ｍ〕 とし、Is set. The initial shape of the manipulator is as shown in FIG. The gain of the controller in the equation (18) is set as * K _P = diag (1700, 1700) [N / m], * K _V = diag (30, 30) [Ns / m],

【００５７】[0057]

【数２４】 (Equation 24)

【００５８】の真値が得られることを仮定し、Assuming that the true value of is obtained,

【００５９】[0059]

【数２５】 (Equation 25)

【００６０】と与える。(17)式、(18)式のコントローラ
により＊τを決定し、(15)式の拘束状態の運動方程式を
ルンゲ・クッタ法で解いた。数値積分の時間刻み幅をΔ
ｔ＝０．００１，０．０００１，０．００００１〔ｓ〕
に設定して、それぞれのThe above is given. * Τ was determined by the controllers of equations (17) and (18), and the equation of motion in the constrained state of equation (15) was solved by the Runge-Kutta method. Δ time step of numerical integration
t = 0.001, 0.0001, 0.00001 [s]
Set to

【００６１】[0061]

【数２６】 (Equation 26)

【００６２】の時間変化を図４に示す。Δｔとｔに拘ら
ずFIG. 4 shows the time variation of the above. Regardless of Δt and t

【００６３】[0063]

【数２７】 [Equation 27]

【００６４】は零であることがわかる。次にｙ−ｙ_d ＝
ｙ_e の時間変化を図５に示す。Δｔに依存してｙ_e が減
少している。Is found to be zero. Then y−y _d =
FIG. 5 shows the time change of y _e . Y _e decreases depending on Δt.

【００６５】[0065]

【数２８】 [Equation 28]

【００６６】であるにも拘らずｙ（ｔ）は零を維持でき
ていない。Δｔが小さいほどｙ_e は零に近づいていくこ
とから、この誤差は(15)式を数値積分するときの積分誤
差が原因と考えられる。以上の積分誤差の評価は、以下
の力／位置制御シミュレーション結果の信頼性を確認す
るためであり、以下のシミュレーションにおいては誤差
がほぼ零となるΔｔ＝０．００００１〔ｓ〕を用いる。Nevertheless, y (t) cannot be maintained at zero. Since y _e approaches zero as Δt is smaller, it is considered that this error is caused by an integration error when numerically integrating equation (15). The above evaluation of the integration error is for confirming the reliability of the following force / position control simulation results. In the following simulation, Δt = 0.00001 [s] at which the error becomes almost zero is used.

【００６７】図６は試験結果を示すグラフであり、FIG. 6 is a graph showing the test results.

【００６８】[0068]

【数２９】 (Equation 29)

【００６９】の真値＊ａ（＊ｘ），＊Ａ（＊ｘ₁ ）が得
られ、（17),(18)式にこれらの真値を用いた場合におけ
るｆ_y −ｆ_d ＝ｆ_c の時間応答を示している。このグラ
フから明らかなようにｆ_y は過渡応答なしにｆ_d と一致
している。しかもこの結果はｆ_d の大きさに無関係であ
ることがわかる。[0069] true value * a of (* x), * A ( * x 1) is _{obtained, (17), f y -f} d = f c in the case of using these true values in (18) FIG. As is apparent from this graph, f _y matches f _d without a transient response. Moreover it can be seen that this result is independent of the magnitude of f _d.

【００７０】[0070]

【発明の効果】第１〜第５の発明にあっては、対象物に
対する所定方向にハンドが拘束された状態で非拘束方向
に対しては自由運動可能なマニピュレータのモデルを求
め、この条件を満たす位置及び速度を求め、拘束力とこ
れを得るための入力との関係式から拘束力ｆを目標値と
するための入力を求めるから、力センサを用いることな
く力制御が可能となり、設備コストの大幅な低減を図れ
る優れた効果を奏する。According to the first to fifth aspects of the present invention, a manipulator model capable of free movement in an unconstrained direction while a hand is constrained in a predetermined direction with respect to an object is obtained. The position and speed to be satisfied are obtained, and the input for setting the constraint force f to the target value is obtained from the relational expression between the constraint force and the input for obtaining the force. Therefore, force control can be performed without using a force sensor, and equipment costs are reduced. An excellent effect can be achieved that can significantly reduce the amount.

【図面の簡単な説明】[Brief description of the drawings]

【図１】本発明を適用する制御系の模式図である。FIG. 1 is a schematic diagram of a control system to which the present invention is applied.

【図２】制御系のブロック線図である。FIG. 2 is a block diagram of a control system.

【図３】シミュレーションモデルの説明図である。FIG. 3 is an explanatory diagram of a simulation model.

【図４】ハンドのｙ軸方向の加速度の時間変化を示す説
明図である。FIG. 4 is an explanatory diagram showing a time change of the acceleration of the hand in the y-axis direction.

【図５】ｙ−ｙ_d ＝ｙ_e の時間変化を示す説明図であ
る。FIG. 5 is an explanatory view showing a time variation of y-y _d = y _e.

【図６】試験結果を示すグラフである。FIG. 6 is a graph showing test results.

【符号の説明】[Explanation of symbols]

１ マニピュレータ ２ 手先効果器 ３ 被加工面 ４，５ リンク ６，７ ジョイント DESCRIPTION OF SYMBOLS 1 Manipulator 2 Hand effector 3 Work surface 4,5 Link 6,7 Joint

Claims (5)

【特許請求の範囲】[Claims] 【請求項１】 対象物の面との間に、これに対して力を
作用させつつ面の接線方向に移動させるという拘束条件
が与えられたハンドを持つマニピュレータの前記力の制
御方法において、ハンドが前記拘束条件を満たすマニピ
ュレータのモデルを求め、該モデルから導いた運動方程
式から前記面に対するハンドの力を算出し、算出した力
を目標値に一致させるに必要なマニピュレータに対する
入力を求め、該入力に基づき前記面に対するハンドの力
を制御することを特徴とするマニピュレータの力制御方
法。1. A force control method for a manipulator having a hand provided with a constraint that a force is applied to a surface of an object and moved in a tangential direction of the surface while applying a force to the surface. Finds a manipulator model that satisfies the constraint conditions, calculates the force of the hand on the surface from the equation of motion derived from the model, finds the input to the manipulator necessary to match the calculated force to the target value, Controlling the force of the hand on the surface based on the force. 【請求項２】 対象物の面との間に、これに対して力を
作用させつつ面の接線方向に移動させるという拘束条件
が与えられたハンドを持つマニピュレータの前記力を検
出する方法において、ハンドが前記拘束条件を満たすマ
ニピュレータのモデルを求め、該モデルから導かれた運
動方程式より前記面に対するハンドの力を算出すること
を特徴とするマニピュレータの力検出方法。2. A method for detecting a force of a manipulator having a hand provided with a constraint condition that a force is applied to a surface of an object and moved in a tangential direction of the surface while applying a force thereto, A method for detecting a force of a manipulator, wherein the hand obtains a model of the manipulator satisfying the constraint condition, and calculates the force of the hand on the surface from an equation of motion derived from the model. 【請求項３】 前記ハンドの力ｆは下式に従って算出す
ることを特徴とするマニピュレータの力検出方法。 【数１】 3. A method for detecting a force of a manipulator, wherein the force f of the hand is calculated according to the following equation. (Equation 1) 【請求項４】 対象物の面との間に、これに対して力を
作用させつつ面の接線方向に移動させるという拘束条件
が与えられたハンドを持つマニピュレータの前記力及び
位置の制御方法において、ハンドが前記拘束条件を満た
すマニピュレータのモデルを求め、該モデルから導いた
運動方程式から前記ハンドの力を演算し、演算したハン
ドの力が目標値に一致し、またハンドの位置が目標軌道
を満足するに必要なマニピュレータに対する入力を演算
し、該入力に基づいてマニピュレータを制御することを
特徴とするマニピュレータの力及び位置制御方法。4. A method for controlling a force and a position of a manipulator having a hand provided with a constraint condition that a force is applied to a surface of an object and moved in a tangential direction of the surface while applying a force thereto. The hand obtains a manipulator model that satisfies the constraint conditions, calculates the force of the hand from the equation of motion derived from the model, the calculated hand force matches the target value, and the position of the hand indicates the target trajectory. A manipulator force and position control method, comprising calculating an input to the manipulator necessary to satisfy the condition, and controlling the manipulator based on the input. 【請求項５】 対象物の面との間に、これに対して力を
作用させつつ面の接線方向に移動させるという拘束条件
が与えられたハンドを持つマニピュレータの力及び位置
を制御する装置において、 対象物に対するハンドの力を演算する手段と、演算した
ハンドの力とハンドの目標拘束力との偏差を解消すべ
く、マニピュレータに対する第１の入力を演算する手段
と、対象物に対するハンドの位置と、対象物に対するハ
ンドの目標軌道との偏差を解消すべくマニピュレータに
対する第２の入力を演算する手段と、前記マニピュレー
タに対する第１、第２の入力の和及び予め求めたマニピ
ュレータのモデル式とに基づいてハンドの設定位置を出
力する手段とを備えることを特徴とするマニピュレータ
の力及び位置制御装置。5. An apparatus for controlling a force and a position of a manipulator having a hand provided with a constraint condition that a force is applied to a surface of an object and moved in a tangential direction of the surface while applying a force thereto. Means for calculating the force of the hand on the object, means for calculating a first input to the manipulator to eliminate a deviation between the calculated hand force and the target binding force of the hand, and a position of the hand relative to the object. Means for calculating a second input to the manipulator to eliminate a deviation of the hand from the target trajectory with respect to the object; and a sum of the first and second inputs to the manipulator and a model formula of the manipulator obtained in advance. Means for outputting a set position of the hand based on the force and the position of the manipulator.