JP2013158837A - Robot control device and robot control method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To control a position and a force of an end effector of a robot in an easier manner.SOLUTION: A robot control device for controlling operations of a robot provided with an arm includes a first acquisition means for acquiring a virtual force to be given to a point of operation of the arm, a first displacement setting means for setting a displacement to which the point of operation moves by giving the acquired virtual force to a first impedance model, and a target position setting means for setting a target position of the point of operation based on the displacement set by the first displacement setting means.

Description

本発明は、ロボット制御装置、およびロボット制御方法に関する。   The present invention relates to a robot control device and a robot control method.

ロボットの運動制御としては、例えば、ロボットアームの手先の位置を制御する位置制御、手先が対象物に加える力を制御する力制御がある。また、位置制御と力制御の両方を統合したハイブリッド制御もある。   Examples of robot motion control include position control for controlling the position of the hand of the robot arm and force control for controlling the force applied by the hand to the object. There is also hybrid control that integrates both position control and force control.

ロボットの力制御としては、例えば、インピーダンス制御が知られている。インピーダンス制御では、例えば、ロボットアームの手先に加わる力やモーメントなどの外力を測定する力覚センサーを用いて、力覚センサーの入力に応じてロボットアームの手先の作業対象物に加わる力を制御する。インピーダンス制御に関する文献としては、特許文献１がある。   As the force control of the robot, for example, impedance control is known. In impedance control, for example, a force sensor that measures an external force such as a force or moment applied to the hand of the robot arm is used to control the force applied to the work target at the hand of the robot arm according to the input of the force sensor. . There is Patent Document 1 as a document relating to impedance control.

特開２００９−６１５５０号公報JP 2009-61550 A

ところで、従来の手先の位置の制御では、例えば、開始位置から目標位置までの軌道を生成し、当該軌道に沿って手先が動くように制御する。また、従来のインピーダンス制御は、力覚センサーに対して加わった力を利用して、手先が対象物に接触した場合の手先の位置を柔軟に対応させるために使用される。従って、インピーダンス制御が働くのは、手先が対象物に接触してからとなる。   By the way, in the conventional control of the position of the hand, for example, a trajectory from the start position to the target position is generated, and control is performed so that the hand moves along the trajectory. Further, the conventional impedance control is used to flexibly correspond to the position of the hand when the hand touches the object using the force applied to the force sensor. Therefore, impedance control works only after the hand touches the object.

ここで、例えば、手先が対象物に接触してから手先を一定の力で対象物に押し付け続ける制御を行う場合、手先の目標位置（「制御点」ともいう。）を手先が対象物を押し付ける方向のどの位置に設定するか、センサー出力に応じたインピーダンス制御による位置修正量はどれくらいの大きさになるか、インピーダンスモデルの係数（慣性、粘性、剛性などの係数）をどのように設定するか、などの要素を考慮して制御する必要があり、処理が複雑である。   Here, for example, when performing control to keep pressing the hand against the object with a constant force after the hand contacts the object, the hand presses the target position of the hand (also referred to as “control point”). Which position in the direction to set, how large is the amount of position correction by impedance control according to the sensor output, and how to set the impedance model coefficients (coefficients of inertia, viscosity, rigidity, etc.) It is necessary to control in consideration of factors such as, and the processing is complicated.

また、従来の位置制御により手先の位置の移動を制御する場合には、例えば、開始位置から目標位置までの軌道上に複数含まれる通過位置間の軌道ごとに、加速区間や減速区間などの速度の設定が行われる。そのため、多くの設定が必要であり、処理が複雑である。   In addition, when the movement of the hand position is controlled by the conventional position control, for example, the speed in the acceleration section, the deceleration section, etc. for each of the trajectories between the passing positions included in the trajectory from the start position to the target position. Is set. Therefore, many settings are necessary and the processing is complicated.

そこで、本発明は、より簡単にロボットのエンドエフェクターの位置制御及び力制御を行うことを目的とする。   Therefore, an object of the present invention is to more easily perform position control and force control of an end effector of a robot.

本願は、上記課題を解決する手段を複数含んでいるが、その例を挙げるならば、以下のとおりである。   The present application includes a plurality of means for solving the above-described problems, and examples thereof are as follows.

上記の課題を解決する本発明の一態様は、アームを備えるロボットの動作を制御するロボット制御装置であって、前記アームの作業点に加える仮想の力を取得する第一の取得手段と、前記取得された仮想の力を第一のインピーダンスモデルに与えることにより、前記作業点が移動すべき変位を設定する第一の変位設定手段と、前記第一の変位設定手段により設定された変位に基づいて前記作業点の目標位置を設定する目標位置設定手段と、を有する、ことを特徴とする。   One aspect of the present invention that solves the above problem is a robot control device that controls the operation of a robot including an arm, the first acquisition means for acquiring a virtual force applied to the work point of the arm, Based on the displacement set by the first displacement setting means, the first displacement setting means for setting the displacement that the working point should move by applying the acquired virtual force to the first impedance model And target position setting means for setting a target position of the work point.

上記の構成によれば、仮想の力をインピーダンスモデルに与えることでエンドエフェクターの移動を制御できるので、より簡単にロボットのエンドエフェクターの位置制御及び力制御を行うことができる。例えば、インピーダンスモデルに仮想力を与えれば、求まる変位に応じて加速や減速などの速度も制御されるので、より少ない設定で簡単に制御が可能である。   According to the above configuration, since the movement of the end effector can be controlled by applying a virtual force to the impedance model, the position control and force control of the end effector of the robot can be performed more easily. For example, if a virtual force is applied to the impedance model, the speeds such as acceleration and deceleration are controlled according to the obtained displacement, so that the control can be easily performed with fewer settings.

ここで、前記ロボット制御装置は、前記作業点に加わった外力を検出するセンサーのセンサー値を取得する第二の取得手段と、前記取得されたセンサー値に対応する外力を第二のインピーダンスモデルに与えることにより、前記作業点が移動すべき変位を設定する第二の変位設定手段と、を有し、前記目標位置設定手段は、前記第一の変位設定手段により設定された変位と、前記第二の変位設定手段により設定された変位と、に基づいて前記作業点の目標位置を設定する、ことを特徴としてもよい。   Here, the robot control device uses a second acquisition unit that acquires a sensor value of a sensor that detects an external force applied to the work point, and an external force corresponding to the acquired sensor value as a second impedance model. A second displacement setting means for setting a displacement to be moved by the working point, and the target position setting means includes the displacement set by the first displacement setting means, and the first displacement setting means. The target position of the work point may be set based on the displacement set by the second displacement setting means.

上記の構成によれば、もう一つのインピーダンスモデルにより外力に応じてエンドエフェクターの移動を制御できるので、仮想の力に応じた移動によりエンドエフェクターが対象物などに接触したときの柔軟な移動を簡単に制御できる。   According to the above configuration, the movement of the end effector can be controlled according to the external force by another impedance model, so that the flexible movement when the end effector comes in contact with the target object can be easily performed by the movement according to the virtual force. Can be controlled.

また、前記第一の取得手段は、前記仮想の力の入力を受け付ける、又は、前記作業点が移動すべき目標変位の入力を受け付けて、当該目標の変位に応じた前記仮想の力を求める、ことにより前記仮想の力を取得する、ことを特徴としてもよい。   Further, the first acquisition means receives the input of the virtual force, or receives an input of a target displacement to which the work point should move, and obtains the virtual force according to the target displacement. In this case, the virtual force may be acquired.

上記の構成によれば、ユーザーは仮想の力又は目標の変位を入力すればよいので、より直感的にロボットの動作（例えば、作業空間でのエンドエフェクターの移動など）を操ることができる。   According to the above configuration, since the user only has to input a virtual force or a target displacement, the operation of the robot (for example, movement of the end effector in the work space) can be manipulated more intuitively.

また、前記第一の取得手段は、前記作業点に加えるべき目標の力の入力を受け付けて、当該目標の力と、前記第二の取得手段により取得されたセンサー値に対応する外力とに基づいて、前記仮想の力を求める、ことにより前記仮想の力を取得する、ことを特徴としてもよい。   The first acquisition unit receives an input of a target force to be applied to the work point, and is based on the target force and an external force corresponding to the sensor value acquired by the second acquisition unit. Then, the virtual force may be obtained by obtaining the virtual force.

上記の構成によれば、ユーザーは目標の力を入力すればよいので、より直感的にロボットの動作（例えば、対象物に対するエンドエフェクターの押し付け動作など）を操ることができる。   According to the above configuration, since the user only has to input a target force, the operation of the robot (for example, the pressing operation of the end effector against the object) can be manipulated more intuitively.

また、前記第一のインピーダンスモデルと前記第二のインピーダンスモデルとは、前記仮想の力と前記外力との方向が相互に逆となるように対称に配置されている、ことを特徴としてもよい。   The first impedance model and the second impedance model may be arranged symmetrically so that directions of the virtual force and the external force are opposite to each other.

上記の構成によれば、エンドエフェクターに加わるベクトル方向の力と、その反対のベクトル方向の力とに対する制御を行うことができる。また、性質（例えば、バネの硬さなど）の異なるモデルを設定することもできるので、エンドエフェクターに加わる力に対する反応（動作）と、その反対の力に対する反応（動作）とを、それぞれ設計することができる。   According to said structure, control with respect to the force of the vector direction added to an end effector and the force of the opposite vector direction can be performed. In addition, models with different properties (for example, spring stiffness) can be set, so the response (motion) to the force applied to the end effector and the response (motion) to the opposite force are designed respectively. be able to.

また、前記第一のインピーダンスモデルは、軸方向の変位、又は軸回りの変位に対応するモデルであり、前記仮想の力は、軸方向の仮想の力、又は軸回りの仮想の力であり、前記第二のインピーダンスモデルは、軸方向の変位、又は軸回りの変位に対応するモデルであり、前記外力は、軸方向の外力、又は軸回りの外力である、ことを特徴としてもよい。   The first impedance model is a model corresponding to axial displacement or axial displacement, and the virtual force is an axial virtual force or an axial virtual force, The second impedance model may be a model corresponding to an axial displacement or an axial displacement, and the external force may be an axial external force or an axial external force.

上記の構成によれば、エンドエフェクターの位置の軸方向の直進運動だけでなく軸回りの回転運動についても、インピーダンスモデルによって制御することができるため、より簡単な制御が可能である。   According to the above configuration, not only the axially straight movement of the position of the end effector but also the rotational movement around the axis can be controlled by the impedance model, so that simpler control is possible.

また、前記ロボット制御装置は、前記第一のインピーダンスモデル及び前記第二のインピーダンスモデルそれぞれの特性の変更を受け付ける受付手段、を有する、ことを特徴としてもよい。   Further, the robot control device may include receiving means for receiving changes in characteristics of the first impedance model and the second impedance model.

上記の構成によれば、簡単にロボットの動作（例えば、対象物に接触した場合の動作や、移動速度など）を調整することができる。   According to said structure, operation | movement of a robot (for example, operation | movement at the time of contacting a target object, a moving speed, etc.) can be adjusted easily.

また、前記ロボット制御装置は、指定された始点から終点までの軌道を生成する軌道生成手段を有し、前記目標位置設定手段は、前記第一の変位設定手段により算出された変位に基づいて前記軌道を修正する、ことを特徴としてもよい。   Further, the robot control device has a trajectory generating means for generating a trajectory from a designated start point to an end point, and the target position setting means is based on the displacement calculated by the first displacement setting means. The trajectory may be corrected.

上記の構成によれば、軌道生成による位置制御とともに仮想の力による動作を制御できる。   According to said structure, the operation | movement by virtual force can be controlled with the position control by orbital generation.

上記の課題を解決する本発明の他の態様は、アームを備えるロボットの動作を制御するロボット制御方法であって、前記アームの作業点に加える仮想の力を取得する第一の取得ステップと、前記取得された仮想の力を第一のインピーダンスモデルに与えることにより、前記作業点が移動すべき変位を設定する第一の変位設定ステップと、前記第一の変位設定ステップにより設定された変位に基づいて前記作業点の目標位置を設定する目標位置設定ステップと、を含む、ことを特徴とする。   Another aspect of the present invention for solving the above problem is a robot control method for controlling the operation of a robot including an arm, the first obtaining step for obtaining a virtual force applied to the work point of the arm; By applying the acquired virtual force to the first impedance model, a first displacement setting step for setting a displacement to be moved by the work point, and a displacement set by the first displacement setting step. A target position setting step for setting a target position of the work point based on the target position.

上記の構成によれば、仮想の力をインピーダンスモデルに与えることでエンドエフェクターの動作を制御できるので、より簡単にロボットの位置制御及び力制御を行うことができる。   According to the above configuration, since the operation of the end effector can be controlled by applying a virtual force to the impedance model, the position control and force control of the robot can be performed more easily.

上記した以外の課題、構成、及び効果は、以下の実施形態の説明により明らかにされる。   Problems, configurations, and effects other than those described above will be clarified by the following description of embodiments.

本発明の一実施形態に係るロボット制御装置の概略構成の一例を示す図である。It is a figure which shows an example of schematic structure of the robot control apparatus which concerns on one Embodiment of this invention. ロボットの概略構成の一例を示す図である。It is a figure which shows an example of schematic structure of a robot. 仮想力制御のインピーダンスモデルと力制御のインピーダンスモデルの関係を説明する図である。It is a figure explaining the relationship between the impedance model of virtual force control, and the impedance model of force control. インピーダンスモデルの一例を示す図である。It is a figure which shows an example of an impedance model. ロボットアームの手先を移動させる制御の流れ一例を示すフロー図である。It is a flowchart which shows an example of the flow of control which moves the hand of a robot arm. ロボットアームの手先を対象物に目標の力で押し付ける制御の流れ一例を示すフロー図である。It is a flowchart which shows an example of the flow of control which presses the hand of a robot arm to a target object with target force.

以下、本発明の一実施形態の一例について、図面を参照して説明する。   Hereinafter, an example of an embodiment of the present invention will be described with reference to the drawings.

図１は、本発明の一実施形態に係るロボット制御装置の概略構成の一例を示す図である。   FIG. 1 is a diagram illustrating an example of a schematic configuration of a robot control apparatus according to an embodiment of the present invention.

ロボット２は、ロボット制御装置１により制御される。ロボット２は、例えば、図２に示すように、ワークの把持などを行うアーム型のロボットである。ロボット２のアームは、例えば、複数のジョイント（関節）２００と、複数のリンク２１０と、ハンド（２２０と、力覚センサー２３０とを備える。   The robot 2 is controlled by the robot control device 1. The robot 2 is, for example, an arm type robot that holds a workpiece as shown in FIG. The arm of the robot 2 includes, for example, a plurality of joints (joints) 200, a plurality of links 210, a hand (220), and a force sensor 230.

各ジョイント２００は、リンク２１０同士や、胴体部とリンク２１０、ハンド２２０とリンク２１０を、回動自在に（ただし、所定の可動範囲内で回動可能に）連結している。本図の例では、ロボット２は、６つのジョイントを備えた６軸アームとなっている。   Each joint 200 connects the links 210 to each other, the body portion and the link 210, and the hand 220 and the link 210 so as to be rotatable (but rotatable within a predetermined movable range). In the example of this figure, the robot 2 is a six-axis arm having six joints.

また、各ジョイント２００やハンド２２０には、それらを動作させるための、例えば、アクチュエーター（不図示）が備えられる。アクチュエーターは、例えば、サーボモーターやエンコーダーなどを備える。エンコーダーが出力するエンコーダー値は、ロボット制御装置１によるロボット２のフィードバック制御に使用される。   Each joint 200 or hand 220 is provided with, for example, an actuator (not shown) for operating them. The actuator includes, for example, a servo motor and an encoder. The encoder value output by the encoder is used for feedback control of the robot 2 by the robot control device 1.

また、ロボット２は、ハンド２に力覚センサー２３０を備えている。力覚センサー２３０は、例えば、手先が障害物などに接触した場合に、当該障害物からの外力（作用力）を計測し、ロボット制御装置１に出力する。力覚センサー２３０が出力するセンサー値は、ロボット制御装置１によるロボット２のインピーダンス制御（従来のインピーダンス制御）に使用される。   The robot 2 includes a force sensor 230 in the hand 2. For example, when the hand touches an obstacle or the like, the force sensor 230 measures an external force (acting force) from the obstacle and outputs it to the robot control device 1. The sensor value output by the force sensor 230 is used for impedance control (conventional impedance control) of the robot 2 by the robot control device 1.

もちろん、上記のロボット２の構成は、本実施形態の特徴を説明するにあたって主要構成を説明したのであって、上記の構成に限られない。一般的な把持ロボットが備える構成を排除するものではない。例えば、図２では６軸のアームが示されているが、軸数（ジョイント数）をさらに増加させてもよいし減らしてもよい。リンクの数を増減させてもよい。また、アーム、ハンド、リンク、ジョイント等の各種部材の形状、大きさ、配置、構造等も適宜変更してよい。力覚センサーの位置なども適宜変更してもよい。   Of course, the configuration of the robot 2 described above is not limited to the configuration described above because the main configuration has been described in describing the features of the present embodiment. This does not exclude the configuration of a general gripping robot. For example, although a 6-axis arm is shown in FIG. 2, the number of axes (number of joints) may be further increased or decreased. The number of links may be increased or decreased. In addition, the shape, size, arrangement, structure, and the like of various members such as the arm, hand, link, and joint may be appropriately changed. The position of the force sensor may be changed as appropriate.

ロボット制御装置１は、ロボット２の動作を制御する装置である。ロボット制御装置１は、制御部１００と、入力部１１０と、サーボ制御部１２０とを備える。   The robot control device 1 is a device that controls the operation of the robot 2. The robot control apparatus 1 includes a control unit 100, an input unit 110, and a servo control unit 120.

制御部１００は、ロボット２の動作制御のための各種演算を行う。制御部１００は、位置制御部１０１と、力制御部１０２と、仮想力制御部１０３と、位置修正部１０４と、インバース・キネマティクス（逆運動学）部１０５と、フォワード・キネマティクス（順運動学）部１０６とを有する。   The control unit 100 performs various calculations for controlling the operation of the robot 2. The control unit 100 includes a position control unit 101, a force control unit 102, a virtual force control unit 103, a position correction unit 104, an inverse kinematics (inverse kinematics) unit 105, and a forward kinematics (forward motion). Study) department 106.

なお、本実施形態では、ハンド２２０の着目したある点を、アームの「作業点（いわゆる、エンドエフェクター）」と呼ぶ。以下では、ハンド２２０の先端（手先）が「作業点（エンドエフェクター）」であるものとして説明する。   In the present embodiment, a certain point of interest of the hand 220 is referred to as an “work point (so-called end effector)” of the arm. In the following description, it is assumed that the tip (hand) of the hand 220 is the “work point (end effector)”.

位置制御部１０１は、従来の位置制御を行う。例えば、位置制御部１０１は、フォワード・キネマティクス部１０６からロボット２の現在の手先の位置を取得する。また、位置制御部１０１は、入力部１１０からロボット２の目標の位置を取得する。そして、位置制御部１０１は、現在の位置（始点）から目標の位置（終点）までの軌道を生成する。   The position control unit 101 performs conventional position control. For example, the position control unit 101 acquires the current hand position of the robot 2 from the forward kinematics unit 106. In addition, the position control unit 101 acquires the target position of the robot 2 from the input unit 110. Then, the position control unit 101 generates a trajectory from the current position (start point) to the target position (end point).

力制御部１０２は、力覚センサー２３０から出力されるセンサー値に基づく従来のインピーダンス制御を行う。例えば、力制御部１０２は、ロボット２の力覚センサー２３０のセンサー値に基づいて、手先に対する外力が計測されている場合に、設定されている所定のインピーダンスモデルを用いて手先の位置の変位（修正量）を算出する。   The force control unit 102 performs conventional impedance control based on the sensor value output from the force sensor 230. For example, when the external force with respect to the hand is measured based on the sensor value of the force sensor 230 of the robot 2, the force control unit 102 uses the predetermined impedance model that is set to change the position of the hand ( (Correction amount) is calculated.

力制御部１０２のインピーダンスモデルは、例えば、図３（手先の左側）に示すように、手先に加わる外力に応じて手先の運動を制御するものである。例えば、手先が障害物などに接触したときに、障害物に対する力（手先に加わる力）を逃がす運動を制御する。なお、力制御のインピーダンスモデルには、例えば、図４に示すような、慣性項、粘性項、及び剛性項を含む一般的な運動方程式を用いることができる。   For example, as shown in FIG. 3 (left side of the hand), the impedance model of the force control unit 102 controls the movement of the hand according to the external force applied to the hand. For example, when the hand touches an obstacle or the like, the movement for releasing the force (force applied to the hand) against the obstacle is controlled. For example, a general equation of motion including an inertia term, a viscosity term, and a stiffness term as shown in FIG. 4 can be used for the impedance model for force control.

仮想力制御部１０３は、力制御のインピーダンスモデルと対称に配置されたインピーダンスモデルにより、仮想力に基づくインピーダンス制御を行う。   The virtual force control unit 103 performs impedance control based on virtual force using an impedance model arranged symmetrically with the impedance model of force control.

なお、本実施形態では、「対称」とは、ある軸方向の力の場合、力の直進方向が相互に逆であることを意味し、ある軸回りの力のモーメント（トルク）の場合、力の回転方向が相互に逆であることを意味する。   In the present embodiment, “symmetry” means that in the case of a force in a certain axial direction, the straight direction of the force is opposite to each other, and in the case of a moment (torque) of a force around a certain axis, This means that the rotation directions are opposite to each other.

例えば、仮想力制御部１０３は、入力部１１０を介してユーザーから、手先の目標の移動量（以下、「目標変位」ともいう。）を受け付ける（取得する）。そして、入力された目標変位を所定の変換式を用いて仮想力に変換し、設定されている所定のインピーダンスモデルを用いて手先の位置の変位（修正量）を算出する。   For example, the virtual force control unit 103 receives (acquires) a target movement amount (hereinafter also referred to as “target displacement”) from the user via the input unit 110. Then, the input target displacement is converted into a virtual force using a predetermined conversion formula, and the displacement (correction amount) of the hand position is calculated using a predetermined impedance model that has been set.

また、例えば、仮想力制御部１０３は、入力部１１０を介してユーザーから、手先に加える目標の力を受け付ける（取得する）。そして、入力された目標の力を所定の変換式を用いて仮想力に変換し、設定されている所定のインピーダンスモデルを用いて手先の位置の変位（修正量）を算出する。なお、目標の力から仮想力への変換を、入力部１１０が行うようにしてもよい。また、入力部１１０が仮想力を受け付けるようにして、変換処理を省略するようにしてもよい。   For example, the virtual force control unit 103 receives (acquires) a target force to be applied to the hand from the user via the input unit 110. Then, the input target force is converted into a virtual force using a predetermined conversion formula, and the displacement (correction amount) of the position of the hand is calculated using a predetermined impedance model that has been set. The input unit 110 may perform conversion from the target force to the virtual force. Further, the conversion process may be omitted by allowing the input unit 110 to accept a virtual force.

仮想力制御部１０３のインピーダンスモデルは、例えば、図３（手先の右側）に示すように、手先に加わる仮想力に応じて手先の運動を制御するものである。例えば、手先を目的の方向に移動させたり、障害物に押し付けたりする運動を制御する。なお、仮想力制御のインピーダンスモデルには、例えば、図４に示すような、慣性項、粘性項、及び剛性項を含む一般的な運動方程式を用いることができる。   The impedance model of the virtual force control unit 103 controls the movement of the hand according to the virtual force applied to the hand, for example, as shown in FIG. 3 (on the right side of the hand). For example, the movement of moving the hand in a target direction or pressing it against an obstacle is controlled. For example, a general equation of motion including an inertia term, a viscosity term, and a stiffness term as shown in FIG. 4 can be used as the impedance model for virtual force control.

位置修正部１０４は、手先の目標位置の修正を行う。例えば、位置制御部１０１による軌道の算出と、力制御部１０２による手先の位置の変位の算出とが行われる場合、位置修正部１０４は、位置制御部１０１から出力される現在位置から目標位置までの軌道に基づく位置指令に対して、力制御部１０２から出力される位置の変位を加えて、位置の修正を行う。なお、力覚センサー２３０により外力が検出されない場合、力制御部１０２の出力に応じた位置の修正は行われない。   The position correcting unit 104 corrects the target position of the hand. For example, when the trajectory is calculated by the position controller 101 and the displacement of the hand position is calculated by the force controller 102, the position corrector 104 moves from the current position output from the position controller 101 to the target position. The position is corrected by adding the displacement of the position output from the force control unit 102 to the position command based on the orbit. When no external force is detected by the force sensor 230, the position is not corrected according to the output of the force control unit 102.

また、例えば、仮想力制御部１０３による手先の位置の変位の算出と、力制御部１０２による手先の位置の変位の算出とが行われる場合、位置修正部１０４は、仮想力制御部１０３から出力される位置の変位に対して、力制御部１０２から出力される位置の変位を加えて、変位の修正を行う。   Further, for example, when the calculation of the displacement of the hand position by the virtual force control unit 103 and the calculation of the displacement of the hand position by the force control unit 102 are performed, the position correction unit 104 outputs from the virtual force control unit 103. The displacement is corrected by adding the displacement of the position output from the force control unit 102 to the displacement of the position to be performed.

インバース・キネマティクス（逆運動学）部１０５は、ロボット２の手先の位置から関節変位を求める。例えば、インバース・キネマティクス（逆運動学）部１０５は、位置修正部１０４から出力される手先の位置を取得し、当該位置を実現する各ジョイント２００の角度情報に変換する。そして、当該角度情報をサーボ制御部１２０に出力する。   An inverse kinematics (inverse kinematics) unit 105 obtains a joint displacement from the position of the hand of the robot 2. For example, the inverse kinematics (inverse kinematics) unit 105 acquires the hand position output from the position correction unit 104 and converts it into angle information of each joint 200 that realizes the position. Then, the angle information is output to the servo control unit 120.

フォワード・キネマティクス（順運動学）部１０６は、ロボット２の関節変位から手先の位置を求める。例えば、フォワード・キネマティクス（順運動学）部１０６は、ロボット２の各ジョイント２００の現在の角度を、サーボ制御部１２０から取得し、この角度情報を手先の位置情報に変換する。そして、当該位置情報を位置制御部１０１に出力する。   The forward kinematics (forward kinematics) unit 106 obtains the position of the hand from the joint displacement of the robot 2. For example, the forward kinematics (forward kinematics) unit 106 acquires the current angle of each joint 200 of the robot 2 from the servo control unit 120, and converts this angle information into hand position information. Then, the position information is output to the position control unit 101.

入力部１１０は、ロボット２の手先の移動の指示を受け付ける。例えば、位置制御部１０１による軌道の算出と、力制御部１０２による手先の位置の変位の算出とが行われる場合、入力部１１０は、ロボット２の手先の目標位置を受け付け、位置制御部１０１に出力する。   The input unit 110 receives an instruction to move the hand of the robot 2. For example, when the trajectory is calculated by the position control unit 101 and the displacement of the hand position is calculated by the force control unit 102, the input unit 110 receives the target position of the hand of the robot 2 and receives the target position in the position control unit 101. Output.

また、例えば、仮想力制御部１０３による手先の位置の変位の算出と、力制御部１０２による手先の位置の変位の算出とが行われる場合、入力部１１０は、ロボット２の手先の目標変位を特定する情報を受け付け、仮想力制御部１０３に出力する。また、上述のように、入力部１１０は、目標の力を特定する情報を受け付けるようにしてもよい。仮想力を特定する情報を受け付けるようにしてもよい。   For example, when the calculation of the displacement of the hand position by the virtual force control unit 103 and the calculation of the displacement of the hand position by the force control unit 102 are performed, the input unit 110 calculates the target displacement of the hand of the robot 2. Information to be identified is received and output to the virtual force control unit 103. Further, as described above, the input unit 110 may receive information for specifying the target force. Information for specifying the virtual force may be received.

なお、入力部１１０は、手先の目標位置や目標変位を、ユーザーから受け付けてもよいし、他のコンピューターや所定のプログラムから受け付けるようにしてもよい。また、入力部１１０は、インピーダンスモデルの特性の変更などを、ユーザー、他のコンピューター、所定のプログラムなどから受け付けるようにしてもよい。   Note that the input unit 110 may receive the target position and target displacement of the hand from the user, or may be received from another computer or a predetermined program. Further, the input unit 110 may accept a change in the characteristics of the impedance model from a user, another computer, a predetermined program, or the like.

サーボ制御部１２０は、ロボットのサーボモーターに指令を送る。例えば、サーボ制御部１２０は、インバース・キネマティクス部１０５から出力された各ジョイント２００の角度情報に基づいて制御指令を生成し、各ジョイント２００のサーボモーターを駆動する。これにより、ロボット２の位置及び姿勢が変化する。また、サーボ制御部１２０は、ロボット２のアクチュエーターからエンコーダー値を取得して、制御部１００に出力する。   The servo control unit 120 sends a command to the servo motor of the robot. For example, the servo control unit 120 generates a control command based on the angle information of each joint 200 output from the inverse kinematics unit 105 and drives the servo motor of each joint 200. As a result, the position and posture of the robot 2 change. In addition, the servo control unit 120 acquires an encoder value from the actuator of the robot 2 and outputs the encoder value to the control unit 100.

上記のロボット制御装置１は、例えば、ＣＰＵと、ＲＡＭなどの主メモリーと、ＨＤＤ（Hard Disk Drive）などの補助記憶装置と、ディスプレイと、マウスやキーボードなどの入力装置と、ネットワークインターフェイス（Ｉ／Ｆ）などの通信装置と、を備えるコンピューターにより実現することができる。制御部１００やサーボ制御部１２０は、専用のＡＳＩＣやコントローラー基板で実現されてもよい。なお、制御部１００やサーボ制御部１２０は、例えば、ＣＰＵがＨＤＤに格納された所定のプログラムをＲＡＭに読み出して実行することにより実現することができる。入力部１１０は、例えば、入力装置により実現される。   The robot control apparatus 1 includes, for example, a CPU, a main memory such as a RAM, an auxiliary storage device such as an HDD (Hard Disk Drive), a display, an input device such as a mouse and a keyboard, and a network interface (I / I). And a communication device such as F). The control unit 100 and the servo control unit 120 may be realized by a dedicated ASIC or a controller board. The control unit 100 and the servo control unit 120 can be realized, for example, when the CPU reads a predetermined program stored in the HDD into the RAM and executes it. The input unit 110 is realized by an input device, for example.

なお、上記に説明した構成は、ロボット制御装置１の構成を理解容易にするために、主な処理内容に応じて分類したものである。構成要素の分類の仕方や名称によって、本願発明が制限されることはない。ロボット制御装置１の構成は、処理内容に応じて、さらに多くの構成要素に分類することもできる。また、１つの構成要素がさらに多くの処理を実行するように分類することもできる。また、各構成要素の処理は、１つのハードウェアで実行されてもよいし、複数のハードウェアで実行されてもよい。   The configuration described above is classified according to the main processing contents in order to facilitate understanding of the configuration of the robot control device 1. The present invention is not limited by the way of classification and names of the constituent elements. The configuration of the robot control device 1 can be classified into more components depending on the processing content. Moreover, it can also classify | categorize so that one component may perform more processes. Further, the processing of each component may be executed by one hardware or may be executed by a plurality of hardware.

次に、上記のロボット制御装置１により実現される特徴的な処理について説明する。   Next, characteristic processes realized by the robot control apparatus 1 will be described.

以下では、仮想力制御部１０３と力制御部１０２による制御について説明する。位置制御部１０１と力制御部１０２による制御については、従来と同様なので説明を省略する。   Hereinafter, control by the virtual force control unit 103 and the force control unit 102 will be described. Since the control by the position control unit 101 and the force control unit 102 is the same as the conventional one, the description thereof is omitted.

図５は、ロボットアームの手先を移動させる制御（位置制御を用いない場合）の流れ一例を示すフロー図である。   FIG. 5 is a flowchart showing an example of a flow of control for moving the hand of the robot arm (when position control is not used).

まず、入力部１１０は、ユーザーの操作により、ロボット２の手先の目標変位の入力を受け付ける（Ｓ１０）。目標変位とは、例えば、手先の現在位置からの、指定方向への直線的な移動量である。   First, the input unit 110 receives an input of a target displacement of the hand of the robot 2 by a user operation (S10). The target displacement is, for example, a linear movement amount in the designated direction from the current position of the hand.

それから、仮想力制御部１０３は、目標変位を仮想力に変換する（Ｓ２０）。具体的には、仮想力制御部１０３は、Ｓ１０で入力された目標変位を、所定の変換式を用いて、仮想力に変換する。所定の変換式は、例えば、Ｆ＝Ｋｘである。ここで、Ｆは仮想力であり、Ｋは剛性項であり、ｘは目標変位である。   Then, the virtual force control unit 103 converts the target displacement into a virtual force (S20). Specifically, the virtual force control unit 103 converts the target displacement input in S10 into a virtual force using a predetermined conversion formula. The predetermined conversion formula is, for example, F = Kx. Here, F is a virtual force, K is a rigidity term, and x is a target displacement.

なお、Ｓ１０において、仮想力を、ユーザーの操作により受け付けるようにしてもよい。この場合、Ｓ２０の処理は、不要である。そして、以降の処理では、入力された仮想力を用いて処理を行えばよい。   In S10, the virtual force may be received by a user operation. In this case, the process of S20 is unnecessary. In subsequent processing, processing may be performed using the input virtual force.

また、仮想力制御部１０３は、Ｓ１２０の終了判定に用いる判定閾値を設定する（Ｓ３０）。判定閾値は、Ｓ６０で仮想力制御部１０３により算出される仮想力に対応する変位が、Ｓ１０で入力された目標変位に十分近付いたことを判定する値である。   Further, the virtual force control unit 103 sets a determination threshold value used for determining whether to end S120 (S30). The determination threshold is a value for determining that the displacement corresponding to the virtual force calculated by the virtual force control unit 103 in S60 is sufficiently close to the target displacement input in S10.

それから、フォワード・キネマティクス部１０６は、各軸の現在位置（エンコーダー値）を取得する（Ｓ４０）。すなわち、ロボット２の各ジョイント２００の現在の角度を、サーボ制御部１２０から取得する。また、フォワード・キネマティクス部１０６は、取得した角度情報を手先の位置情報に変換する（Ｓ５０）。以降の処理（特にＳ９０）では、当該現在の位置情報が、基準の位置情報となる。   Then, the forward kinematics unit 106 acquires the current position (encoder value) of each axis (S40). That is, the current angle of each joint 200 of the robot 2 is acquired from the servo control unit 120. Further, the forward kinematics unit 106 converts the acquired angle information into hand position information (S50). In the subsequent processing (especially S90), the current position information becomes reference position information.

それから、Ｓ６０（仮想力制御部１０３）と、Ｓ７０〜Ｓ８０（力制御部１０２）との処理が、並列的に実行される。もちろん、これらの処理は、順次実行されてもよい。   Then, the processes of S60 (virtual force control unit 103) and S70 to S80 (force control unit 102) are executed in parallel. Of course, these processes may be executed sequentially.

仮想力制御部１０３は、仮想力制御のインピーダンスモデルの運動方程式を解く（Ｓ６０）。具体的には、仮想力制御部１０３は、Ｓ２０で算出した仮想力を、仮想力制御のインピーダンスモデルに加わる力Ｆとして与え、当該運動方程式の解（時間ｔあたりの変位（修正量））を求める。Ｆは仮想力なので、一定である（入力値に固定する）。なお、運動方程式の解を求める方法としては、例えば、ルンゲ・クッタ（Runge-Kutta）法などの一般的なものを用いることができる。 The virtual force control unit 103 solves the equation of motion of the impedance model for virtual force control (S60). Specifically, the virtual force control unit 103 gives the virtual force calculated in S20 as a force F applied to the impedance model of the virtual force control, and gives a solution of the motion equation (displacement per time t (correction amount)). Ask. Since F is a virtual force, it is constant (fixed to the input value). As a method for obtaining the solution of the equation of motion, for example, a general method such as a Runge-Kutta method can be used.

一方、力制御部１０２は、力覚センサー２３０から出力されるセンサー値を取得する（Ｓ７０）。そして、力制御のインピーダンスモデルの運動方程式を解く（Ｓ８０）。具体的には、力制御部１０２は、Ｓ７０で取得したセンサー値に対応する外力を、力制御のインピーダンスモデルに加わる力Ｆとして与え、当該運動方程式の解（時間ｔあたりの変位（修正量））を求める。Ｆはセンサー値なので、不定である。なお、運動方程式の解を求める方法としては、例えば、ルンゲ・クッタ（Runge-Kutta）法などの一般的なものを用いることができる。 On the other hand, the force control unit 102 acquires the sensor value output from the force sensor 230 (S70). Then, the equation of motion of the force control impedance model is solved (S80). Specifically, the force control unit 102 gives an external force corresponding to the sensor value acquired in S70 as a force F applied to the impedance model of force control, and solves the equation of motion (displacement per time t (correction amount)). ) Since F is a sensor value, it is indefinite. As a method for obtaining the solution of the equation of motion, for example, a general method such as a Runge-Kutta method can be used.

なお、外力が検出されていない場合、Ｓ７０〜Ｓ８０の処理は、実行されない。すなわち、力制御部１０２は、外力に応じた手先位置の変位（修正量）を出力しない。   If no external force is detected, the processes of S70 to S80 are not executed. That is, the force control unit 102 does not output the displacement (correction amount) of the hand position according to the external force.

それから、位置修正部１０４は、手先位置の変位の修正を行う（Ｓ９０）。具体的には、位置修正部１０４は、例えば、Ｓ６０で仮想力制御部１０３により算出された位置の変位に対して、Ｓ８０で力制御部１０２により算出された位置の変位を加えて、修正を行う。また、位置修正部１０４は、Ｓ５０で算出された基準の手先位置に、修正後の変位を加えて、目標位置を修正する。   Then, the position correction unit 104 corrects the displacement of the hand position (S90). Specifically, for example, the position correction unit 104 adds the displacement of the position calculated by the force control unit 102 in S80 to the displacement of the position calculated by the virtual force control unit 103 in S60, and corrects it. Do. The position correcting unit 104 corrects the target position by adding the corrected displacement to the reference hand position calculated in S50.

それから、インバース・キネマティクス部１０５は、Ｓ９０で求められた目標位置を、当該位置を実現する各ジョイント２００の角度情報に変換する（Ｓ１００）。また、サーボ制御部１２０は、Ｓ１００で算出された各ジョイント２００の角度情報に基づいて制御指令を生成し、各ジョイント２００のサーボモーターを駆動する（Ｓ１１０）。   Then, the inverse kinematics unit 105 converts the target position obtained in S90 into angle information of each joint 200 that realizes the position (S100). The servo control unit 120 generates a control command based on the angle information of each joint 200 calculated in S100, and drives the servo motor of each joint 200 (S110).

それから、仮想力制御部１０３は、目標変位と仮想力による変位の差分とが判定閾値よりも小さくなったか否かを判定する（Ｓ１２０）。具体的には、仮想力制御部１０３は、Ｓ６０で算出した基準位置からの変位と、Ｓ１０で入力された目標変位との差分を求め、当該差分とＳ３０で設定した判定閾値とを比較する。差分が判定閾値以上の場合（Ｓ１２０：ＮＯ）、処理をＳ５０の直後（Ｓ６０〜Ｓ８０の直前）に戻す。差分が判定閾値より小さい場合（Ｓ１２０：ＹＥＳ）、本フローを終了する。   Then, the virtual force control unit 103 determines whether or not the difference between the target displacement and the displacement due to the virtual force is smaller than the determination threshold (S120). Specifically, the virtual force control unit 103 obtains a difference between the displacement from the reference position calculated in S60 and the target displacement input in S10, and compares the difference with the determination threshold set in S30. When the difference is equal to or larger than the determination threshold (S120: NO), the process is returned to immediately after S50 (immediately before S60 to S80). When the difference is smaller than the determination threshold value (S120: YES), this flow ends.

以上のようにして、指定された変位に基づく仮想力により、手先の移動を制御することができる。例えば、手先に変位を与えれば、仮想力制御のインピーダンスモデルに従って手先が移動する。また、手先が対象物に接触してからは、力制御のインピーダンスモデルに従って、手先に加わる力を逃がす方向にも制御が行われる。   As described above, the movement of the hand can be controlled by the virtual force based on the designated displacement. For example, if a displacement is given to the hand, the hand moves according to the impedance model of virtual force control. Further, after the hand touches the object, control is also performed in a direction in which the force applied to the hand is released according to the impedance model of force control.

なお、位置制御により手先の位置の移動を制御する場合には、例えば、開始位置から目標位置までの軌道上に複数含まれる通過位置間の軌道ごとに、加速区間や減速区間などの速度の設定が行われるため、多くの設定が必要である。これに対して、仮想力制御を用いる場合は、インピーダンスモデルに仮想力を与えれば、求まる変位に応じて加速や減速などの速度も制御されるので、より少ない設定で簡単に制御が可能である。   In addition, when controlling the movement of the position of the hand by position control, for example, setting of speeds such as an acceleration section and a deceleration section for each trajectory between a plurality of passing positions on the trajectory from the start position to the target position. As a result, many settings are required. On the other hand, when virtual force control is used, if virtual force is applied to the impedance model, speeds such as acceleration and deceleration are controlled according to the displacement obtained, so control can be easily performed with fewer settings. .

もちろん、例えば、手先が対象物に接触してからさらに対象物を押すような変位を与えれば、仮想力制御のインピーダンスモデルに従って手先を対象物に押し付けることができる。   Of course, for example, if a displacement is given so as to further push the object after the hand touches the object, the hand can be pressed against the object according to the impedance model of virtual force control.

なお、仮想力制御による変位は、目標変位に対応する仮想力に応じて算出される。一方、力制御による変位は、仮想力制御によって生じた変位によりセンサーが外力を検出し、検出された外力に応じて算出される。ここで、通常センサーの応答速度は速くないため、センサーに対する外力は、仮想力制御による変位の発生から遅れて発生するとともに、仮想力制御による変位が大きくなるに従って大きくなる。そして、最終的に仮想力による変位と外力による変位とが釣り合うことで静止する。従って、手先が対象物に接触してから押し付ける方向に変位を発生させると、対象物を押し付けることができる。   The displacement by the virtual force control is calculated according to the virtual force corresponding to the target displacement. On the other hand, the displacement due to the force control is calculated according to the detected external force by the sensor detecting the external force based on the displacement caused by the virtual force control. Here, since the response speed of the normal sensor is not fast, the external force with respect to the sensor is generated with a delay from the generation of the displacement by the virtual force control, and increases as the displacement by the virtual force control increases. Finally, the displacement due to the virtual force balances with the displacement due to the external force, so that it stops. Therefore, if a displacement is generated in the pressing direction after the hand touches the object, the object can be pressed.

手先と対象物の距離が０であり、かつ力覚センサーの検出する値が０である初期状態から、手先を対象物に押し付ける動作を例に挙げて説明する。   The operation of pressing the hand against the object from the initial state where the distance between the hand and the object is 0 and the value detected by the force sensor is 0 will be described as an example.

例えば、仮想力制御のインピーダンスモデルと、力制御のインピーダンスモデルが同じモデルである場合を考える。仮想力制御のインピーダンスモデルは、バネ係数が１００であり、仮想力５Ｎ（ニュートン）が与えられたときに、０．０５ｍの変位が発生するものとする。力制御のインピーダンスモデルも、同様であるものとする。この場合、仮想力制御のインピーダンスモデルでは、５Ｎの仮想力を与えることで０．０５ｍの変位が発生する。これに対し、力制御のインピーダンスモデルでは、力覚センサーが検出する外力の値が５Ｎになったときに、０．０５ｍの変位が発生する。このように、最終的に力覚センサーが５Ｎの外力を検出したときに、仮想力による変位と外力による変位とが釣り合って手先が静止する。従って、手先の位置は静止したままで、力覚センサーには５Ｎの外力が加わっていることとなる。   For example, consider a case where the impedance model for virtual force control and the impedance model for force control are the same model. The impedance model for virtual force control has a spring coefficient of 100, and a displacement of 0.05 m occurs when a virtual force of 5 N (Newton) is applied. The same applies to the force control impedance model. In this case, in the impedance model for virtual force control, a displacement of 0.05 m is generated by applying a virtual force of 5N. On the other hand, in the impedance model for force control, a displacement of 0.05 m occurs when the value of the external force detected by the force sensor becomes 5N. Thus, when the force sensor finally detects an external force of 5N, the displacement due to the virtual force balances the displacement due to the external force, and the hand rests. Therefore, the position of the hand remains stationary and an external force of 5N is applied to the force sensor.

一方、例えば、仮想力制御のインピーダンスモデルと、力制御のインピーダンスモデルのバネ係数が異なる場合を考える。仮想力制御のインピーダンスモデルは、バネ係数が１００であり、仮想力５Ｎ（ニュートン）が与えられたときに、０．０５ｍの変位が発生するものとする。力制御のインピーダンスモデルは、バネ係数が２００であるものとする。この場合、仮想力制御のインピーダンスモデルでは５Ｎの仮想力を与えることで０．０５ｍの変位が発生する。これに対し、力制御のインピーダンスモデルでは、力覚センサーが検出する外力の値が１０Ｎになったときに、０．０５ｍの変位が発生する。このように、最終的に力覚センサーが１０Ｎの外力を検出したときに、仮想力による変位と外力による変位とが釣り合って手先が静止する。従って、手先の位置は静止したままで、力覚センサーには１０Ｎの外力が加わっていることとなる。   On the other hand, for example, let us consider a case where the spring coefficient of the impedance model for virtual force control and the impedance model for force control are different. The impedance model for virtual force control has a spring coefficient of 100, and a displacement of 0.05 m occurs when a virtual force of 5 N (Newton) is applied. The impedance model for force control is assumed to have a spring coefficient of 200. In this case, in the impedance model for virtual force control, a displacement of 0.05 m is generated by applying a virtual force of 5N. On the other hand, in the impedance model for force control, when the value of the external force detected by the force sensor reaches 10 N, a displacement of 0.05 m occurs. Thus, when the force sensor finally detects an external force of 10 N, the displacement due to the virtual force balances the displacement due to the external force, and the hand rests. Therefore, the position of the hand remains stationary and an external force of 10 N is applied to the force sensor.

また、本実施形態では、仮想力に基づくインピーダンス制御と、外力に基づくインピーダンス制御とが、個別に用意されているので、それぞれに与えられる力に応じて個別に制御を行うことができる。例えば、仮想力制御のインピーダンスモデルによる手先の変位（速度）が、力制御のインピーダンスモデルによる変位（速度）よりも遅くなるように特性を設定（例えば、仮想力制御と力制御の剛性項及び慣性項を同じに設定し、仮想力制御の粘性項を力制御のものよりも大きく設定）すれば、仮想力制御による手先の変位が力制御による手先の変位よりも緩慢となり、手先に過度の力が加わらないようにすることができる。   Further, in the present embodiment, impedance control based on virtual force and impedance control based on external force are prepared separately, so that control can be performed individually according to the force applied to each. For example, characteristics are set so that the displacement (speed) of the hand by the impedance model of virtual force control is slower than the displacement (speed) of the impedance model of force control (for example, the rigidity term and inertia of virtual force control and force control) If the term is set to be the same and the viscosity term for virtual force control is set to be larger than that for force control), the hand displacement caused by virtual force control will be slower than the hand displacement caused by force control, and excessive force will be applied to the hand. Can be added.

なお、位置制御と力制御により手先の位置を制御する場合には、例えば、位置制御による手先の移動の速度に応じて、力制御のインピーダンスモデルの特性を変更しないと、手先や対象物の破損を発生させるおそれがあり、複雑な調整を必要とする。これに対して、仮想力制御を用いる場合は、上述のように設定が簡易である。   In addition, when controlling the position of the hand by position control and force control, for example, if the characteristics of the impedance model of force control are not changed according to the speed of movement of the hand by position control, the hand or object is damaged. May occur, and complicated adjustment is required. On the other hand, when using virtual force control, the setting is simple as described above.

図６は、ロボットアームの手先を目標の力で対象物に押し付ける制御（位置制御を用いない場合）の流れ一例を示すフロー図である。   FIG. 6 is a flowchart showing an example of a flow of control (when position control is not used) for pressing the hand of the robot arm against the object with a target force.

本図は、手先が対象物に向かって移動して接触した後、対象物を一定の力で押し付け続ける場合に適用される（もちろん、対象物に接触しない場合に適用してもよい）。なお、図５においても、指定する変位の値によって、対象物を一定の力で押し付け続ける制御を行うことができるが、図６では、明示的に押し付ける力を入力できるようにしている。図５と異なる点を中心に説明する。   This figure is applied when the hand continues to press the object with a certain force after the hand moves toward the object and comes into contact (of course, it may be applied when the object does not contact the object). In FIG. 5, it is possible to perform control to keep pressing the object with a constant force according to the specified displacement value, but in FIG. 6, the force to be explicitly pressed can be input. A description will be given centering on differences from FIG.

まず、入力部１１０は、ユーザーの操作により、ロボット２の手先に加える目標の力の入力を受け付ける（Ｓ１１）。また、力制御部１０２は、力覚センサー２３０から出力されるセンサー値を取得する（Ｓ１２）。   First, the input unit 110 receives an input of a target force to be applied to the hand of the robot 2 by a user operation (S11). Further, the force control unit 102 acquires the sensor value output from the force sensor 230 (S12).

それから、仮想力制御部１０３は、仮想力を算出する（Ｓ１３）。具体的には、仮想力制御部１０３は、Ｓ１１で入力された目標の力と、Ｓ１２で取得したセンサー値に対応する外力とから、所定の変換式を用いて、仮想力を算出する。所定の変換式は、例えば、Ｆｖ＝（Ｆｐ−Ｆｓ）＊Ｋｖ／Ｋｆである。ここで、Ｆｖは仮想力であり、Ｆｐは目標の力であり、Ｆｓは外力であり、Ｋｖは仮想力制御のインピーダンスモデルの剛性項であり、Ｋｆは力制御のインピーダンスモデルの剛性項である。Ｋｖ／Ｋｆは、二つの剛性項の比を表している。仮想力Ｆｖを仮想力制御に与えることで、目標の力Ｆｐが手先に加わることとなる。   Then, the virtual force control unit 103 calculates a virtual force (S13). Specifically, the virtual force control unit 103 calculates a virtual force using a predetermined conversion formula from the target force input in S11 and the external force corresponding to the sensor value acquired in S12. The predetermined conversion formula is, for example, Fv = (Fp−Fs) * Kv / Kf. Here, Fv is a virtual force, Fp is a target force, Fs is an external force, Kv is a stiffness term of the impedance model of the virtual force control, and Kf is a stiffness term of the impedance model of the force control. . Kv / Kf represents the ratio of two stiffness terms. By applying the virtual force Fv to the virtual force control, the target force Fp is applied to the hand.

それから、仮想力制御部１０３は、目標変位を算出する（Ｓ１４）。具体的には、仮想力制御部１０３は、Ｓ１３で算出した仮想力から、所定の変換式を用いて、目標変位を算出する。所定の変換式は、例えば、ｘ＝Ｆｖ／Ｋｖである。ここで、ｘは目標変位であり、Ｆｖは仮想力であり、Ｋｖは剛性項である。   Then, the virtual force control unit 103 calculates a target displacement (S14). Specifically, the virtual force control unit 103 calculates a target displacement using a predetermined conversion formula from the virtual force calculated in S13. The predetermined conversion formula is, for example, x = Fv / Kv. Here, x is a target displacement, Fv is a virtual force, and Kv is a stiffness term.

また、仮想力制御部１０３は、Ｓ１２０´の終了判定に用いる判定閾値を設定する（Ｓ３０´）。判定閾値は、Ｓ６０´で仮想力制御部１０３により算出される仮想力に対応する変位が、Ｓ１４で算出された目標変位に十分近付いたことを判定する値である。   Further, the virtual force control unit 103 sets a determination threshold value used for determining whether to end S120 ′ (S30 ′). The determination threshold is a value for determining that the displacement corresponding to the virtual force calculated by the virtual force control unit 103 in S60 ′ is sufficiently close to the target displacement calculated in S14.

Ｓ４０、Ｓ５０は、図５の対応する処理と同様なので、説明を省略する。   S40 and S50 are the same as the corresponding processes in FIG.

それから、仮想力制御部１０３は、仮想力制御のインピーダンスモデルの運動方程式を解く（Ｓ６０´）。具体的には、仮想力制御部１０３は、Ｓ１３で算出した仮想力を、仮想力制御のインピーダンスモデルに加わる力Ｆとして運動方程式の解（時間ｔあたりの変位（修正量））を求める。Ｆは仮想力なので、一定である（入力値に固定する）。 Then, the virtual force control unit 103 solves the equation of motion of the impedance model of the virtual force control (S60 ′). Specifically, the virtual force control unit 103 obtains a solution of the equation of motion (displacement (correction amount) per time t) as the force F applied to the impedance model of the virtual force control using the virtual force calculated in S13. Since F is a virtual force, it is constant (fixed to the input value).

Ｓ７０〜Ｓ１１０は、図５の対応する処理と同様なので、説明を省略する。   S70 to S110 are the same as the corresponding processes in FIG.

それから、仮想力制御部１０３は、目標変位と仮想力による変位の差分とが判定閾値よりも小さくなったか否かを判定する（Ｓ１２０´）。具体的には、仮想力制御部１０３は、Ｓ６０´で算出した基準位置からの変位と、Ｓ１４で算出した目標変位との差分を求め、当該差分とＳ３０´で設定した判定閾値とを比較する。差分が判定閾値以上の場合（Ｓ１２０´：ＮＯ）、処理をＳ５０の直後（Ｓ６０´、Ｓ７０〜Ｓ８０の直前）に戻す。差分が判定閾値より小さい場合（Ｓ１２０´：ＹＥＳ）、本フローを終了する。   Then, the virtual force control unit 103 determines whether or not the target displacement and the difference between the displacements due to the virtual force are smaller than the determination threshold (S120 ′). Specifically, the virtual force control unit 103 obtains a difference between the displacement from the reference position calculated in S60 ′ and the target displacement calculated in S14, and compares the difference with the determination threshold set in S30 ′. . When the difference is equal to or greater than the determination threshold (S120 ′: NO), the process is returned to immediately after S50 (immediately before S60 ′ and S70 to S80). When the difference is smaller than the determination threshold (S120 ′: YES), this flow is finished.

以上のようにして、指定された目標の力で手先を対象物に押し付ける動作を制御することができる。例えば、手先が対象物に接触した状態で、手先が対象物に向かう目標の押し付け力を与えれば、仮想力制御のインピーダンスモデルに従った手先の変位により、目標の押し付け力で手先を対象物に押し付けることができる。なお、図５で説明したように、最終的に仮想力と外力とが釣り合うことで静止する。従って、手先が対象物に接触してから押し付ける方向に変位を発生させると、対象物を押し付けることができる。また、仮想力制御及び力制御のインピーダンスモデルに個別に特性を設定できるので、手先に過度の力が加わらないようにすることができる。   As described above, it is possible to control the operation of pressing the hand against the object with the specified target force. For example, if the hand is in contact with the object and gives a target pressing force toward the object, the hand is moved to the object with the target pressing force due to the displacement of the hand according to the impedance model of virtual force control. Can be pressed. As described with reference to FIG. 5, the virtual force and the external force are finally balanced to stop. Therefore, if a displacement is generated in the pressing direction after the hand touches the object, the object can be pressed. Further, since the characteristics can be individually set for the virtual force control and the impedance model for force control, it is possible to prevent excessive force from being applied to the hand.

上述の図５及び図６のフローの各処理単位は、ロボット制御装置１の処理を理解容易にするために、主な処理内容に応じて分割したものである。処理単位の分割の仕方や名称によって、本願発明が制限されることはない。ロボット制御装置１の処理は、処理内容に応じて、さらに多くの処理単位に分割することもできる。また、１つの処理単位がさらに多くの処理を含むように分割することもできる。   Each processing unit in the flow of FIG. 5 and FIG. 6 described above is divided according to main processing contents in order to facilitate understanding of the processing of the robot control device 1. The present invention is not limited by the way of dividing the processing unit or the name. The processing of the robot control apparatus 1 can be divided into more processing units according to the processing content. Moreover, it can also divide | segment so that one process unit may contain many processes.

以上、本発明の一実施形態の一例について説明した。本実施形態によれば、より簡単にロボットの力制御及び位置制御を行うことができる。   Heretofore, an example of an embodiment of the present invention has been described. According to this embodiment, it is possible to more easily perform force control and position control of the robot.

なお、上記の本発明の実施形態は、本発明の要旨と範囲を例示することを意図し、限定するものではない。多くの代替物、修正および変形例が当業者にとって明らかである。   The above-described embodiments of the present invention are intended to illustrate the gist and scope of the present invention and are not intended to be limiting. Many alternatives, modifications, and variations will be apparent to those skilled in the art.

上記の本実施形態の一例では、説明を分かり易くするため、特に変位の方向を限定せずに説明している。当然、複数の軸、例えば、６軸（ＸＹＺ各軸方向の力と、各軸回りの力のモーメント（トルク））のそれぞれについて、力制御のインピーダンスモデルと、それに対称の仮想力制御のインピーダンスモデルを定義すれば、様々な変位に対応できる。   In the example of the present embodiment, the direction of displacement is not particularly limited for easy understanding of the description. Naturally, for each of a plurality of axes, for example, six axes (XYZ axial force and force moment (torque) around each axis), a force control impedance model and a symmetrical virtual force control impedance model are provided. If it is defined, it can cope with various displacements.

すなわち、力制御部１０２は、軸方向の外力（センサー値）が与えられれば、対応するインピーダンスモデルの運動方程式を解いて、与えられた力に応じた変位を算出し、軸回りの外力であるトルク（センサー値）が与えられれば、対応するインピーダンスモデルの運動方程式を解いて、与えられたトルクに応じた変位を算出する。また、仮想力制御部１０３は、軸方向の仮想力が与えられれば、対応するインピーダンスモデルの運動方程式を解いて、与えられた力に応じた変位を算出し、軸回りの仮想力であるトルクが与えられれば、対応するインピーダンスモデルの運動方程式を解いて、与えられたトルクに応じた変位を算出する。   That is, when an external force (sensor value) in the axial direction is given, the force control unit 102 solves the equation of motion of the corresponding impedance model, calculates the displacement according to the given force, and is the external force around the axis. If torque (sensor value) is given, the equation of motion of the corresponding impedance model is solved, and the displacement according to the given torque is calculated. Further, when a virtual force in the axial direction is given, the virtual force control unit 103 solves the equation of motion of the corresponding impedance model, calculates a displacement according to the given force, and a torque that is a virtual force around the axis. Is given, the equation of motion of the corresponding impedance model is solved, and the displacement according to the given torque is calculated.

なお、仮想力による直線的な変位（直進運動）については、上記の実施形態で説明したとおり、仮想力制御部１０３は、例えばＦ＝Ｋｘで求まる仮想力（軸方向の力）を、図４の「直線」の運動方程式のＦに代入すればよい。仮想力による回転の変位（回転運動）については、例えばＴ＝Ｋｒθで求まる仮想力（軸回りのトルク）を、図４の「回転」の運動方程式のＴに代入すればよい。なお、回転の場合、モデルとしてオイラーの運動方程式を用いるようにしてもよい。   As for the linear displacement (straight forward movement) due to the virtual force, as described in the above embodiment, the virtual force control unit 103 uses the virtual force (axial force) obtained by F = Kx, for example, as shown in FIG. Substituting it into F of the “linear” equation of motion. Regarding the displacement of rotation (rotational motion) by the virtual force, for example, the virtual force (torque around the axis) obtained by T = Krθ may be substituted into T of the “rotation” motion equation of FIG. In the case of rotation, Euler's equation of motion may be used as a model.

また、例えば、仮想力制御部１０３と、位置制御部１０１とを併用してもよい。この場合、位置修正部１０４は、位置制御部１０１から出力される現在位置から目標位置までの軌道に基づく位置指令に対して、仮想力制御部１０３から出力される位置の変位を加えて、位置の修正を行う。   For example, the virtual force control unit 103 and the position control unit 101 may be used in combination. In this case, the position correction unit 104 adds the displacement of the position output from the virtual force control unit 103 to the position command based on the trajectory from the current position to the target position output from the position control unit 101, Make corrections.

このようにすれば、例えば、一定の力で対象物を押さえ付けながら手先を移動する、といった動作の制御を行う場合に、位置制御部１０１のみで軌道計画を立てるよりも簡単に軌道計画を立てることができる。すなわち、位置制御部１０１のみで上記のような動作を制御する場合、例えば、手先を対象物に押さえ付けるベクトルへの移動と、手先を移動させたいベクトルへの移動との両方を考慮して、軌道計画を行う必要がある。これに対して、位置制御部１０１と仮想力制御部１０３の両方を用いた場合、手先を対象物に押さえ付けるベクトルへの移動については、仮想力制御部１０３を用いて制御すればよい。従って、位置制御部１０１による軌道計画は簡単になる。   In this way, for example, when controlling the operation of moving the hand while pressing the object with a constant force, the trajectory plan is set up more easily than the trajectory plan is set only by the position control unit 101. be able to. That is, when controlling the above-described operation only by the position control unit 101, for example, considering both the movement to the vector that presses the hand against the object and the movement to the vector to move the hand, It is necessary to conduct trajectory planning. On the other hand, when both the position control unit 101 and the virtual force control unit 103 are used, the movement to the vector that presses the hand against the object may be controlled using the virtual force control unit 103. Therefore, the trajectory planning by the position control unit 101 is simplified.

なお、ロボット制御装置１及びロボット２の機能分担は、上記の実施形態の例に限られず、例えば、ロボット制御装置１の一部分又は全ての機能をロボット２が実行するようにしてもよい。また、例えば、ロボット制御装置１が、ロボット２に含まれていてもよい。   Note that the division of functions between the robot control device 1 and the robot 2 is not limited to the example in the above embodiment, and for example, the robot 2 may execute a part or all of the functions of the robot control device 1. Further, for example, the robot control apparatus 1 may be included in the robot 2.

１：ロボット制御装置、２：ロボット、１００：制御部、１０１：位置制御部、１０２：力制御部、１０３：仮想力制御部、１０４：位置修正部、１０５：インバース・キネマティクス部、１０６：フォワード・キネマティクス部、１１０：入力部、１２０：サーボ制御部、２００：ジョイント、２１０：リンク、２２０：ハンド、２３０：力覚センサー 1: Robot control device, 2: Robot, 100: Control unit, 101: Position control unit, 102: Force control unit, 103: Virtual force control unit, 104: Position correction unit, 105: Inverse kinematics unit, 106: Forward kinematics unit, 110: input unit, 120: servo control unit, 200: joint, 210: link, 220: hand, 230: force sensor

Claims (9)

アームを備えるロボットの動作を制御するロボット制御装置であって、
前記アームの作業点に加える仮想の力を取得する第一の取得手段と、
前記取得された仮想の力を第一のインピーダンスモデルに与えることにより、前記作業点が移動すべき変位を設定する第一の変位設定手段と、
前記第一の変位設定手段により設定された変位に基づいて前記作業点の目標位置を設定する目標位置設定手段と、を有する、
ことを特徴とするロボット制御装置。 A robot control device for controlling the operation of a robot having an arm,
First acquisition means for acquiring a virtual force applied to the work point of the arm;
A first displacement setting means for setting a displacement to which the work point should move by giving the acquired virtual force to the first impedance model;
Target position setting means for setting a target position of the work point based on the displacement set by the first displacement setting means,
A robot controller characterized by that. 請求項１に記載のロボット制御装置であって、
前記作業点に加わった外力を検出するセンサーのセンサー値を取得する第二の取得手段と、
前記取得されたセンサー値に対応する外力を第二のインピーダンスモデルに与えることにより、前記作業点が移動すべき変位を設定する第二の変位設定手段と、を有し、
前記目標位置設定手段は、前記第一の変位設定手段により設定された変位と、前記第二の変位設定手段により設定された変位と、に基づいて前記作業点の目標位置を設定する、
ことを特徴とするロボット制御装置。 The robot control device according to claim 1,
Second acquisition means for acquiring a sensor value of a sensor for detecting an external force applied to the work point;
Second displacement setting means for setting a displacement to which the working point should move by giving an external force corresponding to the acquired sensor value to a second impedance model;
The target position setting means sets the target position of the work point based on the displacement set by the first displacement setting means and the displacement set by the second displacement setting means;
A robot controller characterized by that. 請求項２に記載のロボット制御装置であって、
前記第一の取得手段は、前記仮想の力の入力を受け付ける、又は、前記作業点が移動すべき目標変位の入力を受け付けて、当該目標の変位に応じた前記仮想の力を求める、ことにより前記仮想の力を取得する、
ことを特徴とするロボット制御装置。 The robot control device according to claim 2,
The first acquisition means receives the input of the virtual force, or receives the input of a target displacement to which the work point should move, and obtains the virtual force according to the displacement of the target. Obtaining the virtual force,
A robot controller characterized by that. 請求項２又は３に記載のロボット制御装置であって、
前記第一の取得手段は、前記作業点に加えるべき目標の力の入力を受け付けて、当該目標の力と、前記第二の取得手段により取得されたセンサー値に対応する外力とに基づいて、前記仮想の力を求める、ことにより前記仮想の力を取得する、
ことを特徴とするロボット制御装置。 The robot control device according to claim 2 or 3,
The first acquisition unit receives an input of a target force to be applied to the work point, and based on the target force and an external force corresponding to the sensor value acquired by the second acquisition unit, Obtaining the virtual force by obtaining the virtual force,
A robot controller characterized by that. 請求項２〜４いずれか一項に記載のロボット制御装置であって、
前記第一のインピーダンスモデルと前記第二のインピーダンスモデルとは、前記仮想の力と前記外力との方向が相互に逆となるように対称に配置されている、
ことを特徴とするロボット制御装置。 The robot control device according to any one of claims 2 to 4,
The first impedance model and the second impedance model are arranged symmetrically so that directions of the virtual force and the external force are opposite to each other,
A robot controller characterized by that. 請求項２〜５いずれか一項に記載のロボット制御装置であって、
前記第一のインピーダンスモデルは、軸方向の変位、又は軸回りの変位に対応するモデルであり、
前記仮想の力は、軸方向の仮想の力、又は軸回りの仮想の力であり、
前記第二のインピーダンスモデルは、軸方向の変位、又は軸回りの変位に対応するモデルであり、
前記外力は、軸方向の外力、又は軸回りの外力である、
ことを特徴とするロボット制御装置。 The robot control device according to any one of claims 2 to 5,
The first impedance model is a model corresponding to an axial displacement or a displacement around an axis,
The virtual force is a virtual force in the axial direction or a virtual force around the axis,
The second impedance model is a model corresponding to an axial displacement or a displacement around an axis,
The external force is an external force in the axial direction or an external force around the axis.
A robot controller characterized by that. 請求項２〜６いずれか一項に記載のロボット制御装置であって、
前記第一のインピーダンスモデル及び前記第二のインピーダンスモデルそれぞれの特性の変更を受け付ける受付手段、を有する、
ことを特徴とするロボット制御装置。 The robot control device according to any one of claims 2 to 6,
Receiving means for receiving a change in characteristics of each of the first impedance model and the second impedance model;
A robot controller characterized by that. 請求項１〜７いずれか一項に記載のロボット制御装置であって、
指定された始点から終点までの軌道を生成する軌道生成手段を有し、
前記目標位置設定手段は、前記第一の変位設定手段により算出された変位に基づいて前記軌道を修正する、
ことを特徴とするロボット制御装置。 The robot control device according to any one of claims 1 to 7,
Having a trajectory generating means for generating a trajectory from a specified start point to an end point;
The target position setting means corrects the trajectory based on the displacement calculated by the first displacement setting means;
A robot controller characterized by that. アームを備えるロボットの動作を制御するロボット制御方法であって、
前記アームの作業点に加える仮想の力を取得する第一の取得ステップと、
前記取得された仮想の力を第一のインピーダンスモデルに与えることにより、前記作業点が移動すべき変位を設定する第一の変位設定ステップと、
前記第一の変位設定ステップにより設定された変位に基づいて前記作業点の目標位置を設定する目標位置設定ステップと、を含む、
ことを特徴とするロボット制御方法。 A robot control method for controlling the operation of a robot having an arm,
A first acquisition step of acquiring a virtual force applied to the working point of the arm;
A first displacement setting step of setting a displacement to which the working point should move by applying the acquired virtual force to a first impedance model;
A target position setting step of setting a target position of the working point based on the displacement set by the first displacement setting step,
A robot control method characterized by the above.

Images