JP2017205835A - Control device of manipulator device, control method of the manipulator device, and control program of the manipulator device - Google Patents

Control device of manipulator device, control method of the manipulator device, and control program of the manipulator device Download PDF

Info

Publication number
JP2017205835A
JP2017205835A JP2016099918A JP2016099918A JP2017205835A JP 2017205835 A JP2017205835 A JP 2017205835A JP 2016099918 A JP2016099918 A JP 2016099918A JP 2016099918 A JP2016099918 A JP 2016099918A JP 2017205835 A JP2017205835 A JP 2017205835A
Authority
JP
Japan
Prior art keywords
unit
moving
drive
manipulator device
driven
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
JP2016099918A
Other languages
Japanese (ja)
Inventor
小嶋 晃
Akira Kojima
晃 小嶋
慎吾 松下
Shingo Matsushita
慎吾 松下
高史 西藤
Takashi Nishifuji
高史 西藤
章洋 武捨
Akihiro Takesute
章洋 武捨
山田 淳
Atsushi Yamada
淳 山田
正忠 佐藤
Masatada Sato
正忠 佐藤
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ricoh Co Ltd
Original Assignee
Ricoh Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Ricoh Co Ltd filed Critical Ricoh Co Ltd
Priority to JP2016099918A priority Critical patent/JP2017205835A/en
Publication of JP2017205835A publication Critical patent/JP2017205835A/en
Pending legal-status Critical Current

Links

Abstract

PROBLEM TO BE SOLVED: To perform direct teaching of a manipulator device by improving operability.MEANS FOR SOLVING THE PROBLEM: A control method of a manipulator device, in which an intersection of a plurality of connected moving parts and an intersection of a support part for supporting the moving parts and the moving parts are driven respectively, calculates an estimate value of external force applied on the moving parts based on a change of a movement state of the moving parts at the intersection, controls a drive part for moving the moving parts based on the estimate value of the external force calculated at a timing in which the movement state changes, transmits power of the drive part to a driven part which is driven by the power of the drive part and drives at the intersection, and drives the driven part so that the moving parts move by being driven by the power of the drive part controlled based on the estimate value.SELECTED DRAWING: Figure 1

Description

本発明は、マニピュレータ装置の制御装置、マニピュレータ装置の制御方法及びマニピュレータ装置の制御プログラムに関する。   The present invention relates to a manipulator device control device, a manipulator device control method, and a manipulator device control program.

近年、製造工程を自動化させるファクトリーオートメーションにおいては、本来、人が行う動作や作業をロボットアーム等のマニピュレータ装置に行わせることで製造品質の向上や製造工程を容易に切り替える分散プロセス化が行われている。   In recent years, factory automation that automates the manufacturing process has been performed by a distributed process that easily improves manufacturing quality and easily switches manufacturing processes by manipulator devices such as robot arms that perform operations and operations performed by humans. Yes.

このようなマニピュレータ装置は、ティーチングプレイバック(教示作業)という方法で動作する。このティーチングプレイバックには、操作盤やタブレット端末等の入力装置を用いて、マニピュレータ装置の動作を指定する間接教示と、マニピュレータ装置の先端を模した装置やハンドルをユーザが操作して動作を指定する直接教示とがある。   Such a manipulator device operates by a method called teaching playback (teaching work). This teaching playback uses an input device such as an operation panel or a tablet terminal to specify indirect teaching to specify the operation of the manipulator device, and to specify the operation by operating the device or handle imitating the tip of the manipulator device. There is direct teaching.

直接教示においては、マニピュレータ装置を小さな力で直接教示できるようにモータを駆動させるアシスト制御が行われる。マニピュレータ装置の駆動方法のひとつとして、マニピュレータ装置の関節部分から離れた位置に設置したモータを駆動させ、プーリとベル
トもしくはワイヤによって駆動力を伝達させてマニピュレータ装置を関節ごとに駆動させる方法がある(例えば、特許文献1参照)。
In direct teaching, assist control is performed to drive the motor so that the manipulator device can be directly taught with a small force. As one of the driving methods of the manipulator device, there is a method of driving a manipulator device for each joint by driving a motor installed at a position distant from a joint portion of the manipulator device and transmitting a driving force by a pulley and a belt or a wire ( For example, see Patent Document 1).

しかし、特許文献1に開示された技術は、マニピュレータ装置を動作させるために、加えられた力(外力)を検知したことをトリガとして、アシスト制御が開始される。従って、直接教示を行うためにマニピュレータ装置を動かす際に、装置の重量や大きさによっては、大きな力を加える必要がある。   However, in the technique disclosed in Patent Document 1, assist control is started with the detection of an applied force (external force) as a trigger for operating the manipulator device. Therefore, when the manipulator device is moved for direct teaching, it is necessary to apply a large force depending on the weight and size of the device.

本発明は、上記実情を考慮してなされたものであり、操作性を向上させてマニピュレータ装置の直接教示を行うことを目的とする。   The present invention has been made in consideration of the above circumstances, and an object thereof is to directly teach a manipulator device with improved operability.

上記課題を解決するために、本発明の一態様は、連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御装置であって、前記交差箇所における前記移動部の移動状況の変化に基づいて前記移動部に加わる外力の推定値を算出する外力算出部と、前記移動部を移動させるための駆動部を制御する駆動制御部と、前記交差箇所において前記駆動部の動力に従動して駆動する従動部と、前記駆動部の動力を前記従動部に伝達する駆動伝達部と、を含み、前記駆動制御部は、前記移動状況が変化したタイミングに算出された前記外力の推定値に基づいて前記駆動部を制御し、前記従動部は、前記外力の推定値に基づいて制御される前記駆動部の動力に従動して前記移動部が移動するように駆動することを特徴とする。   In order to solve the above problems, an aspect of the present invention is to control a manipulator device that drives a crossing point of a plurality of connected moving units and a crossing point of a supporting unit that supports the moving unit and the moving unit, respectively. An apparatus that controls an external force calculation unit that calculates an estimated value of an external force applied to the moving unit based on a change in a moving state of the moving unit at the intersection, and a drive unit that moves the moving unit A drive control unit, a driven unit driven and driven by the power of the drive unit at the intersection, and a drive transmission unit that transmits the power of the drive unit to the driven unit. The drive unit is controlled based on the estimated value of the external force calculated at the timing when the movement state changes, and the driven unit is driven by the power of the drive unit controlled based on the estimated value of the external force. Move There and drives to move.

本発明によれば、直接教示を行う際に、操作性を向上させてマニピュレータ装置の直接教示を行うことが出来る。   According to the present invention, when direct teaching is performed, operability can be improved and direct teaching of the manipulator device can be performed.

本発明の実施形態に係るマニピュレータ装置の機械構成図。The machine block diagram of the manipulator apparatus which concerns on embodiment of this invention. 本発明の実施形態に係るワイヤ駆動部の基礎構成図。The basic block diagram of the wire drive part which concerns on embodiment of this invention. 本発明の実施形態に係るワイヤ駆動部の基礎構成分解図。The basic composition exploded view of the wire drive part concerning the embodiment of the present invention. 本発明の実施形態に係る制御装置のハードウェア構成を示すブロック図。The block diagram which shows the hardware constitutions of the control apparatus which concerns on embodiment of this invention. 本発明の実施形態に係る制御装置の機能構成を示すブロック図。The block diagram which shows the function structure of the control apparatus which concerns on embodiment of this invention. 本発明の実施形態に係る外力算出部の内部構成を示す機能ブロック図。The functional block diagram which shows the internal structure of the external force calculation part which concerns on embodiment of this invention. 本発明の実施形態に係る駆動制御部の機能構成を示すブロック図。The block diagram which shows the function structure of the drive control part which concerns on embodiment of this invention. 本発明の実施形態に係るマニピュレータ装置において発生する力について説明する図。The figure explaining the force which generate | occur | produces in the manipulator apparatus which concerns on embodiment of this invention. 本発明の実施形態に係るマニピュレータ装置の動作を直接教示する際の動作を例示した図。The figure which illustrated operation | movement at the time of teaching directly the operation | movement of the manipulator apparatus which concerns on embodiment of this invention. 本発明の実施形態に係るマニピュレータ装置のアシスト制御の説明図。Explanatory drawing of the assist control of the manipulator apparatus which concerns on embodiment of this invention. 本発明の実施形態に係るマニピュレータ装置のアシスト制御の流れを示すフローチャート。The flowchart which shows the flow of the assist control of the manipulator apparatus which concerns on embodiment of this invention. 本発明の実施形態に係るマニピュレータ装置に設定される移動力目標値を示す図。The figure which shows the moving force target value set to the manipulator apparatus which concerns on embodiment of this invention. 本発明の実施形態に係るマニピュレータ装置のアシスト制御の説明図。Explanatory drawing of the assist control of the manipulator apparatus which concerns on embodiment of this invention. 本発明の実施形態に係るひずみゲージを示す図。The figure which shows the strain gauge which concerns on embodiment of this invention. 本発明の実施形態に係る張力調整部を示す図。The figure which shows the tension | tensile_strength adjustment part which concerns on embodiment of this invention. 本発明の実施形態に係る回転ダンパの構成を示す図。The figure which shows the structure of the rotary damper which concerns on embodiment of this invention. 本発明の実施形態に係る移動方向検知部の構成を示す図。The figure which shows the structure of the moving direction detection part which concerns on embodiment of this invention.

以下、図面を参照して、本発明の実施形態を詳細に説明する。本実施形態においては、自由度が「2」である垂直多関節型のロボットアームを例として、マニピュレータ装置の関節にあたる駆動部での動作を補助するアシスト制御について説明する。   Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In the present embodiment, assist control for assisting the operation of the drive unit corresponding to the joint of the manipulator device will be described by taking a vertical articulated robot arm having a degree of freedom of “2” as an example.

図1は、本実施形態に係るマニピュレータ装置1の機械構成図である。図1に示すように、本実施形態に係るマニピュレータ装置1は、第一関節トルクリミッタ171、第二関節トルクリミッタ172、第一関節軸エンコーダ173、第二関節軸エンコーダ174、第一関節181、第二関節182、第一リンク183、第二リンク184、第三リンク185、第一ワイヤ186、第二ワイヤ187、第一モータ188、第一モータエンコーダ189、第一モータ減速装置190、第二モータ191、第二モータエンコーダ192、第二モータ減速装置193、第一駆動プーリ194、第一従動プーリ195、第二駆動プーリ196、第二中継プーリ197、第二従動プーリ198、ベース199を含む。   FIG. 1 is a mechanical configuration diagram of a manipulator device 1 according to the present embodiment. As shown in FIG. 1, the manipulator device 1 according to the present embodiment includes a first joint torque limiter 171, a second joint torque limiter 172, a first joint axis encoder 173, a second joint axis encoder 174, a first joint 181, Second joint 182, first link 183, second link 184, third link 185, first wire 186, second wire 187, first motor 188, first motor encoder 189, first motor speed reducer 190, second Includes a motor 191, a second motor encoder 192, a second motor speed reducer 193, a first drive pulley 194, a first driven pulley 195, a second drive pulley 196, a second relay pulley 197, a second driven pulley 198, and a base 199. .

第一リンク183、第二リンク184、第三リンク185は、その交差箇所を、夫々第一関節181、第二関節182を回転軸とした回転可能な機構によって支持され、第一モータ188、第二モータ191によって移動させられる移動部として機能する。また、ベース199は、第一リンク183、第二リンク184、第三リンク185を支持する支持部として機能する。第二関節182によって支持されていない方の第三リンク185の端部には、把持動作を行うピッキングハンドやエアーチャック等のリンク機構が備えられている。   The first link 183, the second link 184, and the third link 185 are supported at their intersections by a rotatable mechanism having the first joint 181 and the second joint 182 as rotation axes, respectively. It functions as a moving unit that is moved by the two motors 191. The base 199 functions as a support portion that supports the first link 183, the second link 184, and the third link 185. At the end of the third link 185 that is not supported by the second joint 182, a link mechanism such as a picking hand or an air chuck that performs a gripping operation is provided.

また、リンク機構と第三リンク185との接続部分には、マニピュレータ装置1が移動したことを検知する移動検知センサ112が含まれる構成であってもよい。この移動検知センサ112が含まれる場合、後述する外力検知部218と連動して機能する。   Further, the connection portion between the link mechanism and the third link 185 may include a movement detection sensor 112 that detects that the manipulator device 1 has moved. When this movement detection sensor 112 is included, it functions in conjunction with an external force detection unit 218 described later.

尚、移動検知センサ112として、六軸力覚センサ等を用いる構成であってもよい。本実施形態に係るマニピュレータ装置1は、図4で説明する制御装置2が出力する制御信号に基づいて動作を制御される。   Note that a configuration using a six-axis force sensor or the like may be used as the movement detection sensor 112. The operation of the manipulator device 1 according to the present embodiment is controlled based on a control signal output from the control device 2 described in FIG.

まず、第一モータ188による第一リンク183の駆動態様について説明する。第一リンク183には駆動部である第一モータ188が取り付けられており、第一モータ188の回転動作を制御する機構としてエンコーダ189が同軸上に取り付けられている。また、第一リンク183には、第一モータ188と同軸上に第一モータ減速装置190が取り付けられ、第一モータ減速装置190に第一駆動プーリ194が取り付けられている。   First, a driving mode of the first link 183 by the first motor 188 will be described. A first motor 188 as a drive unit is attached to the first link 183, and an encoder 189 is coaxially attached as a mechanism for controlling the rotation operation of the first motor 188. A first motor reduction gear 190 is attached to the first link 183 coaxially with the first motor 188, and a first drive pulley 194 is attached to the first motor reduction device 190.

一方で、第一リンク183は第一関節181を回転軸として、第一従動プーリ195によって回動可能に支持されている。第一駆動プーリ194及び第一従動プーリ195には駆動伝達部である第一ワイヤ186が巻きつけられている。   On the other hand, the first link 183 is rotatably supported by a first driven pulley 195 with the first joint 181 as a rotation axis. A first wire 186 that is a drive transmission unit is wound around the first drive pulley 194 and the first driven pulley 195.

また、第一関節181には一定のトルクを保持可能にするために、トルクリミッタ171が取り付けられている。そのため、トルクリミッタ171は、第二リンク184が第一関節181を回転軸として回転駆動する際に負荷として作用する、落下防止ブレーキとしても機能する。   In addition, a torque limiter 171 is attached to the first joint 181 so that a constant torque can be maintained. Therefore, the torque limiter 171 also functions as a fall prevention brake that acts as a load when the second link 184 is rotationally driven with the first joint 181 as the rotation axis.

尚、図1の構成では出力軸である第一関節181にトルクリミッタ171が取り付けられているが、第一モータ188にトルクリミッタあるいは電磁ブレーキが取り付けられる構成であってもよい。   In the configuration of FIG. 1, the torque limiter 171 is attached to the first joint 181 that is the output shaft, but a configuration in which a torque limiter or an electromagnetic brake is attached to the first motor 188 may be used.

また、第一関節181にはさらに光学式の第一関節軸エンコーダ173が取り付けられている。第一関節軸エンコーダ173は、回転検知部として機能する。第一関節軸エンコーダ173は、ワイヤの伸び等を考慮して第一関節181に取り付けられる。尚、第一ワイヤ186を十分な張力で張り、ワイヤの伸びが発生しなければ、第一モータエンコーダ189と第一関節軸エンコーダ173とは線形性を有するため、第一関節軸エンコーダ173は不要である。   Further, an optical first joint axis encoder 173 is further attached to the first joint 181. The first joint axis encoder 173 functions as a rotation detection unit. The first joint axis encoder 173 is attached to the first joint 181 in consideration of wire elongation and the like. If the first wire 186 is stretched with sufficient tension and the wire does not stretch, the first motor encoder 189 and the first joint axis encoder 173 have linearity, so the first joint axis encoder 173 is unnecessary. It is.

以上説明したように、第一モータ188による駆動力は、第一駆動プーリ194から第一ワイヤ186に伝達され、第一ワイヤ186が第一関節181を水平回転軸として第一従動プーリ195を回転駆動させる。そして、第一従動プーリ195は、第一従動プーリ195に固定されている第二リンク184を、第一関節181を水平回転軸として垂直に回転駆動させる。   As described above, the driving force by the first motor 188 is transmitted from the first driving pulley 194 to the first wire 186, and the first wire 186 rotates the first driven pulley 195 with the first joint 181 as the horizontal rotation axis. Drive. The first driven pulley 195 rotates the second link 184 fixed to the first driven pulley 195 in the vertical direction with the first joint 181 as the horizontal rotation axis.

次に第二モータ191による第一リンク183の駆動態様について説明する。第一リンク183には駆動部である第二モータ191が取り付けられており、第二モータ191の回転動作を制御する機構としてエンコーダ192が同軸上に取り付けられている。また、第一リンク183には、第二モータ191と同軸上に第二モータ減速装置193が取り付けられ、第二モータ減速装置193に第二駆動プーリ196が取り付けられている。   Next, a driving mode of the first link 183 by the second motor 191 will be described. A second motor 191 which is a drive unit is attached to the first link 183, and an encoder 192 is coaxially attached as a mechanism for controlling the rotation operation of the second motor 191. In addition, a second motor speed reduction device 193 is attached to the first link 183 coaxially with the second motor 191, and a second drive pulley 196 is attached to the second motor speed reduction device 193.

一方で、第一リンク183は第一関節181を回転軸として、第二中継プーリ197によっても回転可能に支持されている。また、第二リンク184は第二関節182を回転軸として、第二従動プーリ198によって回転可能に支持されている。第二駆動プーリ196、第二中継プーリ197、第二従動プーリ198には、夫々駆動伝達部である第二ワイヤ187が巻きつけられている。   On the other hand, the first link 183 is also rotatably supported by the second relay pulley 197 with the first joint 181 as a rotation axis. The second link 184 is rotatably supported by the second driven pulley 198 with the second joint 182 as a rotation axis. A second wire 187 that is a drive transmission portion is wound around the second drive pulley 196, the second relay pulley 197, and the second driven pulley 198.

また、第二関節182には一定のトルクを保持可能にするためにトルクリミッタ172が取り付けられている。そのため、トルクリミッタ172は、第三リンク185が第二関節182を回転軸として回転駆動する際に負荷として作用する、落下防止ブレーキとしても機能する。   In addition, a torque limiter 172 is attached to the second joint 182 so that a constant torque can be maintained. Therefore, the torque limiter 172 also functions as a fall prevention brake that acts as a load when the third link 185 is rotationally driven with the second joint 182 as a rotation axis.

尚、図1の構成では出力軸である第二関節182にトルクリミッタ172が取り付けられているが、第二モータ191にトルクリミッタあるいは電磁ブレーキが取り付けられる構成であってもよい。   In the configuration of FIG. 1, the torque limiter 172 is attached to the second joint 182 as the output shaft. However, a configuration in which a torque limiter or an electromagnetic brake is attached to the second motor 191 may be used.

また、第二関節182にはさらに光学式の第二関節軸エンコーダ174が取り付けられている。第二関節軸エンコーダ174は、回転検知部として機能する。第二関節軸エンコーダ174は、ワイヤの伸び等を考慮して第二関節182に取り付けられる。尚、第二ワイヤ187を十分な張力で張り、ワイヤの伸びが発生しなければ、第二モータエンコーダ192と第二関節軸エンコーダ174は線形性を有するため、第二関節軸エンコーダ174は不要である。   An optical second joint axis encoder 174 is further attached to the second joint 182. The second joint axis encoder 174 functions as a rotation detection unit. The second joint axis encoder 174 is attached to the second joint 182 in consideration of wire elongation and the like. If the second wire 187 is stretched with sufficient tension and the wire does not stretch, the second motor encoder 192 and the second joint axis encoder 174 have linearity, so the second joint axis encoder 174 is not necessary. is there.

以上説明したように、第二モータ191による駆動力は、第二駆動プーリ196から第二ワイヤ187に伝達され、第二ワイヤ187が第二関節182を水平回転軸として第二従動プーリ198を回転駆動させる。そして、第二従動プーリ198は、第二従動プーリ198に固定されている第三リンク185を、第二関節182を水平回転軸として垂直に回転駆動させる。   As described above, the driving force by the second motor 191 is transmitted from the second driving pulley 196 to the second wire 187, and the second wire 187 rotates the second driven pulley 198 with the second joint 182 as the horizontal rotation axis. Drive. The second driven pulley 198 rotates the third link 185 fixed to the second driven pulley 198 vertically with the second joint 182 as a horizontal rotation axis.

次に、図2及び図3を参照して、本実施形態に係るワイヤ駆動部の構成について説明する。図2は、本実施形態に係るワイヤ駆動部の基礎構成図、図3は、本実施形態に係るワイヤ駆動部の基礎構成分解図である。   Next, the configuration of the wire drive unit according to the present embodiment will be described with reference to FIGS. 2 and 3. FIG. 2 is a basic configuration diagram of the wire driving unit according to the present embodiment, and FIG. 3 is an exploded basic configuration diagram of the wire driving unit according to the present embodiment.

図2に示すように、第一駆動プーリ194及び第一従動プーリ195には駆動伝達部として第一ワイヤ186が巻きつけられている。第一ワイヤ186は図2において時計回りの回転を伝達する186A部と、反時計回りの回転を伝達する186B部とに分かれて巻き付けられている。   As shown in FIG. 2, a first wire 186 is wound around the first drive pulley 194 and the first driven pulley 195 as a drive transmission unit. In FIG. 2, the first wire 186 is wound separately into a 186A portion that transmits clockwise rotation and a 186B portion that transmits counterclockwise rotation.

また、第一駆動プーリ194は第一ワイヤ186の186A部と186B部を夫々受ける194A部と194B部とで構成されている。さらに、第一従動プーリ195は第一ワイヤ186の186A部と186B部を夫々受ける195A部と195B部とで構成されている。   The first drive pulley 194 includes a 194A portion and a 194B portion that receive the 186A portion and the 186B portion of the first wire 186, respectively. Further, the first driven pulley 195 is composed of a 195A portion and a 195B portion that receive the 186A portion and the 186B portion of the first wire 186, respectively.

図3に示すように、第一ワイヤ186の端部には、固定部材201Aおよび201Bが取り付けられている。この固定部材201Aおよび201Bを第一駆動プーリの溝194A、194Bに夫々組み込むことで第一ワイヤ186は第一駆動プーリ194に固定される。以上の構成は第二駆動プーリ196と、その関連部についても同様であるため、重複する説明を省略する。   As shown in FIG. 3, fixing members 201 </ b> A and 201 </ b> B are attached to the end of the first wire 186. The first wire 186 is fixed to the first drive pulley 194 by incorporating the fixing members 201A and 201B in the grooves 194A and 194B of the first drive pulley, respectively. Since the above configuration is the same for the second drive pulley 196 and its related parts, a duplicate description is omitted.

図4は、本実施形態に係るマニピュレータ装置1を制御する制御装置2のハードウェア構成を示すブロック図である。図4に示すように、本実施形態に係る制御装置2は、一般的なサーバやPC(Personal Computer)等の情報処理端末と同様の構成に加えて、モータ等の駆動機構を制御するエンジンを有する。即ち、本実施形態に係る制御装置2は、CPU(Central Processing Unit)21、RAM(Random Access Memory)22、ROM(Read Only Memory)23、エンジン24、HDD(Hard Disk Drive)25及びI/F26がバス29を介して接続されている。I/F26には、センサ27やスイッチ28が接続され、スイッチ28は、マニピュレータ装置1の電源を制御するものである。   FIG. 4 is a block diagram illustrating a hardware configuration of the control device 2 that controls the manipulator device 1 according to the present embodiment. As shown in FIG. 4, the control device 2 according to the present embodiment includes an engine that controls a driving mechanism such as a motor in addition to the same configuration as an information processing terminal such as a general server or a PC (Personal Computer). Have. That is, the control device 2 according to the present embodiment includes a CPU (Central Processing Unit) 21, a RAM (Random Access Memory) 22, a ROM (Read Only Memory) 23, an engine 24, an HDD (Hard Disk Drive) 25, and an I / F 26. Are connected via a bus 29. A sensor 27 and a switch 28 are connected to the I / F 26, and the switch 28 controls the power supply of the manipulator device 1.

また、センサ27は、図1の移動検知センサ112を含み、マニピュレータ装置1の状態を検知するものである。さらに、I/F26にはタブレット端末等の外部装置がネットワークを介して接続され、マニピュレータ装置1の動作に関する設定を入力することが出来る。   The sensor 27 includes the movement detection sensor 112 of FIG. 1 and detects the state of the manipulator device 1. Furthermore, an external device such as a tablet terminal is connected to the I / F 26 via a network, and settings relating to the operation of the manipulator device 1 can be input.

CPU21は演算手段であり、マニピュレータ装置1全体の動作を制御する。RAM22は、情報の高速な読み書きが可能な揮発性の記憶媒体であり、CPU21が情報を処理する際の作業領域として用いられる。ROM23は、読み出し専用の不揮発性記憶媒体であり、ファームウェア等のプログラムが格納されている。エンジン24は、マニピュレータ装置1において実際にモータ駆動を実行する機構である。   The CPU 21 is a calculation means and controls the operation of the entire manipulator device 1. The RAM 22 is a volatile storage medium capable of reading and writing information at high speed, and is used as a work area when the CPU 21 processes information. The ROM 23 is a read-only nonvolatile storage medium and stores a program such as firmware. The engine 24 is a mechanism that actually executes motor drive in the manipulator device 1.

HDD25は、情報の読み書きが可能な不揮発性の記憶媒体であり、OS(Operating System)や各種の制御プログラム、アプリケーション・プログラム等が格納されている。I/F26は、バス29と各種のハードウェアやネットワーク等を接続し制御する。   The HDD 25 is a non-volatile storage medium that can read and write information, and stores an OS (Operating System), various control programs, application programs, and the like. The I / F 26 connects and controls the bus 29 and various hardware and networks.

このようなハードウェア構成において、ROM23に格納されたプログラムや、HDD25若しくは図示しない光学ディスク等の記録媒体からRAM22に読み出されたプログラムに従ってCPU21が演算を行うことにより、ソフトウェア制御部が構成される。このようにして構成されたソフトウェア制御部と、ハードウェアとの組み合わせによって、本実施形態に係る制御装置2の機能を実現する機能ブロックが構成される。   In such a hardware configuration, the software control unit is configured by the CPU 21 performing calculations according to a program stored in the ROM 23 or a program read to the RAM 22 from a recording medium such as the HDD 25 or an optical disk (not shown). . A functional block that realizes the function of the control device 2 according to the present embodiment is configured by a combination of the software control unit configured as described above and hardware.

図5は、本実施形態に係るマニピュレータ装置1の動作機構を示す図である。図1に示したようなマニピュレータ装置1は、制御装置2によって動作を制御される。制御装置2はPC等の演算機能を備えた端末であるため、制御装置2を操作して、マニピュレータ装置1の位置や速度をユーザが任意に設定することが出来る。また、外部装置から制御装置2に入力される命令に基づいてマニピュレータ装置1の動作を制御する構成であってもよい。   FIG. 5 is a diagram illustrating an operation mechanism of the manipulator device 1 according to the present embodiment. Operation of the manipulator device 1 as shown in FIG. 1 is controlled by the control device 2. Since the control device 2 is a terminal having a calculation function such as a PC, the user can arbitrarily set the position and speed of the manipulator device 1 by operating the control device 2. Moreover, the structure which controls operation | movement of the manipulator apparatus 1 based on the command input into the control apparatus 2 from an external device may be sufficient.

第一関節軸エンコーダ173、第二関節軸エンコーダ174は、回転速度や回転位置及び回転方向を検知するためのマーカーが付加された円板を含み、そのマーカーを光学的に読み取ることによって検知信号を出力する。従って、第一関節軸エンコーダ173、第二関節軸エンコーダ174は、移動方向検知部としても機能する。尚、検知信号の出力態様について詳細は後述する。   The first joint axis encoder 173 and the second joint axis encoder 174 include a disk to which markers for detecting the rotation speed, rotation position, and rotation direction are added, and the detection signals are detected by optically reading the markers. Output. Accordingly, the first joint axis encoder 173 and the second joint axis encoder 174 also function as a movement direction detection unit. The output mode of the detection signal will be described later in detail.

第一モータ減速装置190、第二モータ減速装置193は、それぞれ、第一モータ188、第二モータ191の回転数を減少させ、高いトルクを得るための装置である。また、第一モータ188、第二モータ191の回転数とマニピュレータ装置1が必要とする回転数が異なる場合に第一モータ188、第二モータ191の回転数を調節する。   The first motor speed reduction device 190 and the second motor speed reduction device 193 are devices for reducing the rotation speeds of the first motor 188 and the second motor 191 and obtaining high torque, respectively. Moreover, when the rotation speed of the 1st motor 188 and the 2nd motor 191 and the rotation speed which the manipulator apparatus 1 requires differ, the rotation speed of the 1st motor 188 and the 2nd motor 191 is adjusted.

第一モータ188、第二モータ191は、マニピュレータ装置1の動力装置であり、マニピュレータ装置1は第一モータ188、第二モータ191の回転駆動により発生した動力を第一ワイヤ186、第二ワイヤ187によって伝達させ、第一従動プーリ195、第二従動プーリ198を駆動させる。従って、第一モータ188、第二モータ191は第一従動プーリ195、第二従動プーリ198を回転駆動させる駆動部10、11として機能する。   The first motor 188 and the second motor 191 are power devices of the manipulator device 1, and the manipulator device 1 uses the first wire 186 and the second wire 187 to generate power generated by the rotational drive of the first motor 188 and the second motor 191. To drive the first driven pulley 195 and the second driven pulley 198. Accordingly, the first motor 188 and the second motor 191 function as the drive units 10 and 11 that rotationally drive the first driven pulley 195 and the second driven pulley 198.

また、第一ワイヤ186、第二ワイヤ187は、第一モータ188、第二モータ191の動力を伝達する駆動伝達部として機能する。さらに、第一駆動プーリ194、第二駆動プーリ196は、第一モータ188、第二モータ191の動力に基づいて従動する従動部として機能する。   The first wire 186 and the second wire 187 function as a drive transmission unit that transmits the power of the first motor 188 and the second motor 191. Furthermore, the first drive pulley 194 and the second drive pulley 196 function as a driven portion that is driven based on the power of the first motor 188 and the second motor 191.

図5は、本実施形態に係る制御装置2の機能構成を示すブロック図である。図4に示すように、制御装置2は、駆動制御部211、回転駆動部212、FB(フィードバック)取得部215、位置情報取得部216、外力算出部217を含む。尚、マニピュレータ装置1に移動検知センサ112が含まれる構成である場合、制御装置2は、図6に示すように、外力算出部217の内部機能として外力検知部218を含む。本実施形態にかかる制御装置2は、フィードバック制御部として機能し、マニピュレータ装置1は制御装置2から出力された信号によって動作を制御される。   FIG. 5 is a block diagram illustrating a functional configuration of the control device 2 according to the present embodiment. As shown in FIG. 4, the control device 2 includes a drive control unit 211, a rotation drive unit 212, an FB (feedback) acquisition unit 215, a position information acquisition unit 216, and an external force calculation unit 217. When the manipulator device 1 includes the movement detection sensor 112, the control device 2 includes an external force detection unit 218 as an internal function of the external force calculation unit 217, as shown in FIG. The control device 2 according to the present embodiment functions as a feedback control unit, and the operation of the manipulator device 1 is controlled by a signal output from the control device 2.

駆動制御部211は、I/Fを介して外部装置から入力される駆動部10、11の回転速度の目標値、FB取得部215から入力されるフィードバック信号、外力算出部217から入力される外力の推定値に基づき、駆動部10、11を回転させるための制御値を回転駆動部212に対して出力する。回転駆動部212は、駆動制御部211から入力される制御値に基づき、駆動部10、11を回転させるためのPWM信号を生成して出力する。   The drive control unit 211 is a target value of the rotational speed of the drive units 10 and 11 input from the external device via the I / F, a feedback signal input from the FB acquisition unit 215, and an external force input from the external force calculation unit 217. Based on the estimated value, a control value for rotating the driving units 10 and 11 is output to the rotation driving unit 212. The rotation drive unit 212 generates and outputs a PWM signal for rotating the drive units 10 and 11 based on the control value input from the drive control unit 211.

駆動部10、11は、回転駆動部212から入力されるPWM信号に応じて回転する第一モータ188、第二モータ191である。この駆動部10、11の回転駆動によって、第一関節181、第二関節182を回転軸としてマニピュレータ装置1が移動される。駆動部10、11としては、DC(Direct Current)モータを用いることが可能であり、インナーロータ型DCモータであるブラシレスDCモータやブラシ付きDCモータを用いることが可能である。   The drive units 10 and 11 are a first motor 188 and a second motor 191 that rotate according to the PWM signal input from the rotation drive unit 212. The manipulator device 1 is moved about the first joint 181 and the second joint 182 as rotational axes by the rotational drive of the drive units 10 and 11. As the drive units 10 and 11, a DC (Direct Current) motor can be used, and a brushless DC motor that is an inner rotor type DC motor or a brushed DC motor can be used.

本実施形態に係る駆動部10、11は、回転速度や回転位置を検知するためのマーカーが付加された円板を回転させる。また、第一関節181、第二関節182が、駆動部10、11の回転駆動に従動して回転駆動することによって、回転速度や回転位置を検知するためのマーカーが付加された円盤を回転させる。   The drive units 10 and 11 according to the present embodiment rotate a disk to which a marker for detecting a rotation speed and a rotation position is added. Further, the first joint 181 and the second joint 182 are driven to rotate following the rotational drive of the drive units 10 and 11, thereby rotating the disk to which markers for detecting the rotational speed and the rotational position are added. .

そして、第一モータエンコーダ189、第二モータエンコーダ192によって構成される回転検知部214は、そのマーカーを光学的に読み取ることによって検知信号を出力する。また、第一関節軸エンコーダ173、第二関節軸エンコーダ174によって構成される位置変化量検知部219も、円盤に付加されたマーカーを光学的に読み取ることによって検知信号を外力算出部217に出力する。   And the rotation detection part 214 comprised by the 1st motor encoder 189 and the 2nd motor encoder 192 outputs a detection signal by optically reading the marker. The position change amount detection unit 219 configured by the first joint axis encoder 173 and the second joint axis encoder 174 also outputs a detection signal to the external force calculation unit 217 by optically reading a marker added to the disk. .

具体的には、駆動部10、11の回転に伴って回転する円盤に付加されたマーカーが、回転検知部214が光学的に読み取る読み取り位置を通過する。これにより、回転検知部214による光学的な読み取り状態が変化し、回転検知部214がマーカーを検知して検知信号を出力する。尚、位置変化量検知部219においても同様に、第一関節181、第二関節182の回転に伴って回転する円盤に付加されたマーカーが、位置変化量検知部219が光学的に読み取る位置を通過する。   Specifically, the marker added to the disk that rotates as the driving units 10 and 11 rotate passes through the reading position where the rotation detection unit 214 optically reads. Thereby, the optical reading state by the rotation detection unit 214 changes, and the rotation detection unit 214 detects the marker and outputs a detection signal. Similarly, in the position change amount detection unit 219, the marker added to the disk that rotates with the rotation of the first joint 181 and the second joint 182 indicates the position that the position change amount detection unit 219 reads optically. pass.

尚、本実施形態に係る回転検知部214及び位置変化量検知部219は2組のセンサを含む。この2組のセンサは、上述した円板に付加されたマーカーの位相差がπ/2(rad)となるように配置されている。これにより、本実施形態に係る回転検知部214及び位置変化量検知部219は、π/2(rad)の位相差を有する2つの検知信号を出力する。   The rotation detection unit 214 and the position change amount detection unit 219 according to the present embodiment include two sets of sensors. The two sets of sensors are arranged so that the phase difference of the marker added to the disk described above is π / 2 (rad). Thereby, the rotation detection unit 214 and the position change amount detection unit 219 according to the present embodiment output two detection signals having a phase difference of π / 2 (rad).

FB取得部215は、回転検知部214の検知信号を取得し、単位時間あたりにマーカーが検知された回数に基づいて、駆動部10、11の回転位置を計算し、その計算結果から駆動部10、11の回転速度を算出する。算出された駆動部10、11の回転速度は、フィードバック値として駆動制御部211に入力される。また、FB取得部215は、上述したように、π/2(rad)の位相差を有する2つの検知信号の位相差を利用して、駆動部10、11の回転方向を検知する。   The FB acquisition unit 215 acquires the detection signal of the rotation detection unit 214, calculates the rotation position of the drive units 10 and 11 based on the number of times the marker is detected per unit time, and the drive unit 10 based on the calculation result. , 11 is calculated. The calculated rotation speeds of the drive units 10 and 11 are input to the drive control unit 211 as feedback values. Further, as described above, the FB acquisition unit 215 detects the rotation direction of the drive units 10 and 11 using the phase difference between the two detection signals having a phase difference of π / 2 (rad).

このようにして出力されるFB取得部215のフィードバック値に基づき、駆動制御部211は、外部装置から入力される目標値と、FB取得部215から入力されるフィードバック値との差分に基づいて、駆動部10、11を回転させるための制御値を出力する。   Based on the feedback value of the FB acquisition unit 215 output in this way, the drive control unit 211 is based on the difference between the target value input from the external device and the feedback value input from the FB acquisition unit 215. A control value for rotating the drive units 10 and 11 is output.

位置情報取得部216は、上述したような制御装置2におけるフィードバック制御のサイクルにおいて、駆動部10、11の回転状態を示す値である特徴量を取得する。図5において破線で示しているように、駆動部10、11の回転状態を示す値としては、FB取得部215が取得した回転検知部214の検知信号や、FB取得部215が算出した駆動部10、11の回転速度、外力算出部217が算出した第一関節181、第二関節182の回転位置を用いることができる。   The position information acquisition unit 216 acquires a feature amount that is a value indicating the rotation state of the drive units 10 and 11 in the feedback control cycle in the control device 2 as described above. As indicated by broken lines in FIG. 5, values indicating the rotation state of the drive units 10 and 11 include the detection signal of the rotation detection unit 214 acquired by the FB acquisition unit 215 and the drive unit calculated by the FB acquisition unit 215. 10 and 11 and the rotation positions of the first joint 181 and the second joint 182 calculated by the external force calculation unit 217 can be used.

また駆動制御部211が回転駆動部212に入力する制御値を用いることができる。更に、回転駆動部212が出力するPWM信号を用いることができる。これらの値が、フィードバック制御において得られるフィードバック値である。位置情報取得部216は、取得したマニピュレータ装置1の位置情報を駆動制御部211に返信する。   Further, a control value input by the drive control unit 211 to the rotation drive unit 212 can be used. Furthermore, a PWM signal output from the rotation drive unit 212 can be used. These values are feedback values obtained in the feedback control. The position information acquisition unit 216 returns the acquired position information of the manipulator device 1 to the drive control unit 211.

外力算出部217は、位置変化量検知部219の検知信号を取得し、単位時間あたりにマーカーが検知された回数に基づいて、第一関節181、第二関節182の回転位置を算出する。尚、この計算結果から、第一関節181、第二関節182の回転速度を算出する構成であってもよい。   The external force calculation unit 217 acquires the detection signal of the position change amount detection unit 219, and calculates the rotational positions of the first joint 181 and the second joint 182 based on the number of times the marker is detected per unit time. In addition, the structure which calculates the rotational speed of the 1st joint 181 and the 2nd joint 182 from this calculation result may be sufficient.

以上説明したように、外力算出部217は、第一従動プーリ195、第二従動プーリ198に設置された第一関節軸エンコーダ173、第二関節軸エンコーダ174から送信される信号に基づいて、マニピュレータ装置1に加えられている外力の推定値を算出する。   As described above, the external force calculation unit 217 is based on the signals transmitted from the first joint axis encoder 173 and the second joint axis encoder 174 installed in the first driven pulley 195 and the second driven pulley 198. An estimated value of the external force applied to the device 1 is calculated.

マニピュレータ装置1のユーザがマニピュレータ装置1を動かそうとした場合、第一関節軸エンコーダ173、第二関節軸エンコーダ174は、それぞれ第一関節181、第二関節182における位置の変化量を検知する。この時、第一関節軸エンコーダ173、第二関節軸エンコーダ174において検知される位置の変化量とは、第一関節181と第二関節182夫々の関節軸に対して第一リンク183及び第二リンク184、第二リンク184と第三リンク185が夫々形成する角度の変化量のことを示す。   When the user of the manipulator device 1 tries to move the manipulator device 1, the first joint axis encoder 173 and the second joint axis encoder 174 detect the amount of change in position at the first joint 181 and the second joint 182, respectively. At this time, the change amounts of the positions detected by the first joint axis encoder 173 and the second joint axis encoder 174 are the first link 183 and the second link with respect to the joint axes of the first joint 181 and the second joint 182, respectively. The amount of change in angle formed by the link 184, the second link 184, and the third link 185 is shown.

外力算出部217は、第一関節軸エンコーダ173、第二関節軸エンコーダ174が位置の変化量を検知したタイミングにおいて、その位置の変化量に基づいて、マニピュレータ装置1に加えられている外力の推定値を算出する。   The external force calculation unit 217 estimates the external force applied to the manipulator device 1 based on the position change amount at the timing when the first joint axis encoder 173 and the second joint axis encoder 174 detect the position change amount. Calculate the value.

尚、マニピュレータ装置1の先端部分に移動検知センサ112が設置されている場合に、制御装置2が外力検知部218を含む構成であってもよい。外力検知部218は、移動検知センサ112によって第二リンク184、第三リンク185に力が加えられたこと(外力)を検知すると、第一モータ188、第二モータ191への入力トルクやマニピュレータ装置1の位置及び速度等の情報を取得する。そして取得した情報に基づいて外力を検知する。   In addition, when the movement detection sensor 112 is installed in the front-end | tip part of the manipulator apparatus 1, the structure containing the external force detection part 218 may be sufficient as the control apparatus 2. When the external force detection unit 218 detects that the force is applied to the second link 184 and the third link 185 (external force) by the movement detection sensor 112, the input torque to the first motor 188 and the second motor 191 and the manipulator device Information such as position and speed of 1 is acquired. And external force is detected based on the acquired information.

また、制御装置2に外力検知部218が含まれる構成である場合、外力算出部217が、外力検知部218で検知された外力の値に基づいてマニピュレータ装置1に加えられている外力の推定値を算出する構成であってもよい。   When the control device 2 includes the external force detection unit 218, the external force calculation unit 217 estimates the external force applied to the manipulator device 1 based on the external force value detected by the external force detection unit 218. May be configured to calculate.

尚、本実施形態に係るフィードバック制御においては、第一関節181、第二関節182を回転駆動させるために、駆動制御部211が、駆動部10、11の回転速度及び第一関節181、第二関節182の位置の目標値とフィードバック値とに基づいて制御値を出力する。即ち、回転速度に基づいた制御(以降、「速度制御」とする)が行われる。また、第一関節181、第二関節182の回転位置に基づいた制御(以降、「位置制御」とする)が行われる。第二リンク184、第三リンク185が移動されることで加わる外力と、装置自体の回転速度及び位置情報に基づいて、マニピュレータ装置1の動作を制御することが本実施形態に係る要旨の1つである。   In the feedback control according to the present embodiment, the drive control unit 211 rotates the first joint 181 and the second joint 182 to rotate the first joint 181 and the second joint 181. A control value is output based on the target value of the position of the joint 182 and the feedback value. That is, control based on the rotation speed (hereinafter referred to as “speed control”) is performed. Control based on the rotational positions of the first joint 181 and the second joint 182 (hereinafter referred to as “position control”) is performed. One aspect of the present embodiment is to control the operation of the manipulator device 1 based on the external force applied by moving the second link 184 and the third link 185, and the rotation speed and position information of the device itself. It is.

位置制御を行う場合、FB取得部215は、マーカーが検知された回数に基づいて駆動部10、11の回転位置を計算し、フィードバック値として駆動制御部211に入力する。また、駆動制御部211には、第一関節181、第二関節182の回転位置の目標値が入力される。これにより、駆動制御部211は、目標値とフィードバック値との差分に基づいて駆動部10、11の回転を制御するための制御値を生成して出力する。これにより、第一関節181、第二関節182の回転位置が、入力された目標値に保たれる。   When performing position control, the FB acquisition unit 215 calculates the rotational position of the drive units 10 and 11 based on the number of times the marker is detected, and inputs the calculated rotation position to the drive control unit 211 as a feedback value. Further, the target values of the rotational positions of the first joint 181 and the second joint 182 are input to the drive control unit 211. Thereby, the drive control part 211 produces | generates and outputs the control value for controlling rotation of the drive parts 10 and 11 based on the difference of a target value and a feedback value. Thereby, the rotational positions of the first joint 181 and the second joint 182 are maintained at the input target values.

このように、駆動部10、11に従動して回転駆動する第一関節181と第二関節182は、制御装置2によってその回転駆動を制御される。   As described above, the rotation drive of the first joint 181 and the second joint 182 that are driven to rotate following the drive units 10 and 11 is controlled by the control device 2.

図7は、本実施形態に係る制御装置2の特徴的な機能を実現する駆動制御部211の機能構成を示す図である。図7に示すように、駆動制御部211は、移動力目標生成部221、位置速度指令生成部222を含む。移動力目標生成部221は、制御装置2に入力されるマニピュレータ装置1を移動させる移動力を設定する情報に基づいて、第二リンク184、第三リンク185を移動させる時の移動力目標値の設定を行う。また、マニピュレータ装置1の位置や外力の変化から、移動力目標値の設定を変更して第一関節181、第二関節182を回転させるために駆動部10、11の駆動を制御するための移動力目標情報を生成する。   FIG. 7 is a diagram illustrating a functional configuration of the drive control unit 211 that realizes a characteristic function of the control device 2 according to the present embodiment. As shown in FIG. 7, the drive control unit 211 includes a moving force target generation unit 221 and a position / speed command generation unit 222. The moving force target generation unit 221 sets the moving force target value when moving the second link 184 and the third link 185 based on the information for setting the moving force for moving the manipulator device 1 input to the control device 2. Set up. Moreover, the movement for controlling the drive of the drive units 10 and 11 to change the setting of the moving force target value and rotate the first joint 181 and the second joint 182 from the change of the position of the manipulator device 1 or the external force. Force target information is generated.

位置速度指令生成部222は、制御装置2に入力されるマニピュレータ装置1の移動範囲や移動速度を設定する情報に基づいて、第二リンク184、第三リンク185を移動させる位置速度目標値の設定を行う。また、マニピュレータ装置1の位置や移動速度の変化と目標値との差分に基づいて、第一関節181、第二関節182を回転させるために駆動部10、11の駆動を制御しマニピュレータ装置1の位置・移動速度の設定を変更するための位置速度指令情報を生成する。   The position / speed command generation unit 222 sets position / speed target values for moving the second link 184 and the third link 185 based on information for setting the movement range and movement speed of the manipulator device 1 input to the control device 2. I do. Further, based on the difference between the change in the position and moving speed of the manipulator device 1 and the target value, the drive of the drive units 10 and 11 is controlled to rotate the first joint 181 and the second joint 182, and the manipulator device 1 Position / speed command information for changing the position / movement speed setting is generated.

図8(a)及び(b)は、本実施形態に係るマニピュレータ装置1において発生する力について説明する図である。本実施形態においては、第二リンク184、第三リンク185が第一関節181、第二関節182で接続された2関節マニピュレータの平面運動と第三リンク185の自由末端における力Fとの関係から、第一関節トルクリミッタ171、第二関節トルクリミッタ172に加わるトルクを算出する。   FIGS. 8A and 8B are views for explaining the force generated in the manipulator device 1 according to the present embodiment. In the present embodiment, from the relationship between the planar motion of the two-joint manipulator in which the second link 184 and the third link 185 are connected by the first joint 181 and the second joint 182 and the force F at the free end of the third link 185. The torque applied to the first joint torque limiter 171 and the second joint torque limiter 172 is calculated.

図8(a)は、本実施形態に係るマニピュレータ装置1のxy平面での動作を模式的に示した図である。ここでは、第二関節182の第二関節トルクリミッタ172によって第二リンク184と連結されていない側の第三リンク185の末端が自由末端となる。実際のマニピュレータ装置1の動作においては、この自由末端には、移動検知センサ112が取り付けられ、外力の検知を行う構成であってもよい。   Fig.8 (a) is the figure which showed typically the operation | movement in xy plane of the manipulator apparatus 1 which concerns on this embodiment. Here, the end of the third link 185 on the side not connected to the second link 184 by the second joint torque limiter 172 of the second joint 182 becomes a free end. In the actual operation of the manipulator device 1, the movement detection sensor 112 may be attached to the free end to detect an external force.

図8(b)は、図8(a)にて示したマニピュレータ装置1に発生する物理量を説明する図である。図8(b)においては、夫々、Ii:慣性モーメント、Li:リンクの長さ、mi:質量、θi:角度、τi:トルク、ri:関節中心と質量中心との距離、g:重力加速度(y軸負方向)を示している。また、本実施形態においては、自由度2のマニピュレータ装置1を例として説明するので、i=1,2とする。   FIG. 8B is a diagram for explaining physical quantities generated in the manipulator device 1 shown in FIG. In FIG. 8B, Ii: moment of inertia, Li: link length, mi: mass, θi: angle, τi: torque, ri: distance between joint center and mass center, g: gravitational acceleration ( y-axis negative direction). In the present embodiment, since the manipulator device 1 having two degrees of freedom will be described as an example, i = 1, 2.

図8(b)に示すように、マニピュレータ装置1がxy平面で動作している時の運動方程式は、(数1)で与えられる。
As shown in FIG. 8B, the equation of motion when the manipulator device 1 is operating on the xy plane is given by (Equation 1).

数1において、qは一般化座標を示し、qは(数2)のようにあらわされる。
(数1)において、第一項は慣性力、第二項はコリオリ力、第三項は重力項を示す。
In Equation 1, q indicates a generalized coordinate, and q is expressed as (Equation 2).
In (Equation 1), the first term represents inertial force, the second term represents Coriolis force, and the third term represents gravity term.

(数3)に示すように、本実施形態に係るマニピュレータ装置1に(数1)に示す運動方程式を適用すると、動作中に第一関節トルクリミッタ171、第二関節トルクリミッタ172に加わるトルクを推定することが出来る。
As shown in (Equation 3), when the equation of motion shown in (Equation 1) is applied to the manipulator device 1 according to the present embodiment, torque applied to the first joint torque limiter 171 and the second joint torque limiter 172 during operation is calculated. Can be estimated.

(数3)で推定したトルクは、マニピュレータ装置1の機械構成や減速装置、剛性、粘性摩擦等を考慮することで更に精度を高めることが出来る。また、数3で推定したトルクと、第三リンク185の自由末端における力Fとには、(数4)に示す関係がある。
Jはヤコビ行列を表す。従って、第三リンク185の自由末端における力Fは、
によって求めることが出来る。
The torque estimated by (Equation 3) can be further improved in accuracy by taking into account the mechanical configuration of the manipulator device 1, the speed reduction device, rigidity, viscous friction, and the like. Further, the torque estimated in Equation 3 and the force F at the free end of the third link 185 have the relationship shown in (Equation 4).
J represents a Jacobian matrix. Therefore, the force F at the free end of the third link 185 is
Can be obtained.

また、本実施形態に係るマニピュレータ装置1のヤコビ行列は、
である。(数6)を(数5)に代入すると、
となる。このようにして、マニピュレータ装置1の動作から第三リンク185の自由末端における力Fを求めることが出来る。この時求めた力Fは、第三リンク185に加えられた外力に相当する。
The Jacobian matrix of the manipulator device 1 according to this embodiment is
It is. Substituting (Equation 6) into (Equation 5),
It becomes. In this way, the force F at the free end of the third link 185 can be obtained from the operation of the manipulator device 1. The force F obtained at this time corresponds to the external force applied to the third link 185.

次に、図9を参照して本実施形態に係るマニピュレータ装置1を直接教示する際の動作について説明する。図9は、本実施形態に係るマニピュレータ装置1の動作を直接教示する際にユーザが行う動作を例示した図である。本実施形態に係るマニピュレータ装置1のように、マニピュレータ装置の関節部およびその駆動方式として、関節の回転軸と同じ軸にモータおよび減速装置を配置しているものがある。   Next, with reference to FIG. 9, the operation | movement at the time of teaching the manipulator apparatus 1 which concerns on this embodiment directly is demonstrated. FIG. 9 is a diagram illustrating an operation performed by the user when directly teaching the operation of the manipulator device 1 according to the present embodiment. As in the manipulator device 1 according to the present embodiment, there are some in which a motor and a speed reducer are arranged on the same axis as the rotation axis of the joint as a joint portion of the manipulator device and a driving method thereof.

このようなマニピュレータ装置においては、減速装置の出力軸にリンク部などの次節移動部を直接接続することで、簡単な構造かつ軽量な構成で関節としての機能を実現している。また、この減速装置として、多段遊星歯車機構や波動歯車装置を用いたマニピュレータ装置があるが、直接教示を行う際に、装置自体の質量や減速装置のギヤ効率、内部摩擦などによってマニピュレータ装置が動き出すまでに大きな力を加える必要がある。   In such a manipulator device, a function as a joint is realized with a simple structure and a light weight configuration by directly connecting a next-node moving portion such as a link portion to the output shaft of the reduction gear. In addition, there is a manipulator device using a multistage planetary gear mechanism or a wave gear device as the speed reducer. However, when performing direct teaching, the manipulator device starts to move due to the mass of the device itself, the gear efficiency of the speed reducer, internal friction, etc. It is necessary to apply a great force until.

図9に示すように、マニピュレータ装置1を直接教示する際に、上述したような理由によってマニピュレータ装置1を移動させることが困難な場合がある。そこで、本実施形態に係るマニピュレータ装置1においては、外力が検知される前に、マニピュレータ装置1が教示される方向に働く力(移動量)を検知する。また、移動量が検知されたことをトリガとしてマニピュレータ装置1のアシスト制御を開始することにより、直接教示を行う方向にいち早く力が付与されるため、マニピュレータ装置1移動状況に応じて、操作性を向上させた直接教示を行うことが可能になる。   As shown in FIG. 9, when teaching the manipulator device 1 directly, it may be difficult to move the manipulator device 1 for the reasons described above. Therefore, in the manipulator device 1 according to the present embodiment, the force (movement amount) acting in the direction in which the manipulator device 1 is taught is detected before the external force is detected. Further, by starting assist control of the manipulator device 1 triggered by the detection of the amount of movement, a force is quickly applied in the direction of direct teaching, so that the operability can be improved according to the movement state of the manipulator device 1. Improved direct teaching can be performed.

このように、ユーザによるマニピュレータ装置1の移動を補助するために、駆動部10、11に対して行う制御のことを以後、「アシスト制御」と記載する。図9及び図10に示すように、ユーザが直接教示する方向にマニピュレータ装置1のアシスト制御を行い、操作性を高めて直接教示を行うことが本発明の要旨の1つである。   In this way, the control performed on the drive units 10 and 11 in order to assist the user in moving the manipulator device 1 is hereinafter referred to as “assist control”. As shown in FIG. 9 and FIG. 10, it is one of the gist of the present invention that the assist control of the manipulator device 1 is performed in the direction directly taught by the user, and the direct teaching is performed with improved operability.

次に、図11を参照して、本実施形態に係るマニピュレータ装置1のアシスト制御の流れについて説明する。図11は、本実施形態に係るマニピュレータ装置1が移動される際にアシスト制御を行う処理の流れを示すフローチャートである。位置変化量検知部219において位置の変化量が検知される(S1101)と、外力算出部217は、第二リンク184、第三リンク185が移動されることで発生した位置の変化量であるか否か判定する(S1102)。   Next, with reference to FIG. 11, the flow of assist control of the manipulator device 1 according to the present embodiment will be described. FIG. 11 is a flowchart showing a flow of processing for performing assist control when the manipulator device 1 according to the present embodiment is moved. When the position change amount detection unit 219 detects a position change amount (S1101), the external force calculation unit 217 is a position change amount generated by moving the second link 184 and the third link 185. It is determined whether or not (S1102).

検知された位置の変化量が、外力算出部217において、第二リンク184、第三リンク185が操作されて発生した位置の変化ではないと判定された場合(S1102/No)、駆動制御部211は、本処理を終了させる。外力算出部217は、例えば、マニピュレータ装置1がベース199ごと動かされて生じた位置の変化量については、第二リンク184、第三リンク185が操作されて発生した位置の変化ではないと判定する。   When it is determined in the external force calculation unit 217 that the detected change in position is not a change in position generated by operating the second link 184 and the third link 185 (S1102 / No), the drive control unit 211. Terminates this process. For example, the external force calculation unit 217 determines that the change amount of the position generated when the manipulator device 1 is moved together with the base 199 is not the change of the position generated when the second link 184 and the third link 185 are operated. .

検知された位置の変化量が、外力算出部217において、第二リンク184、第三リンク185が操作されて発生した位置の変化であると判定された場合(S1102/Yes)、駆動制御部211は、検知された位置の変化量に基づいて第二リンク184、第三リンク185が移動するように第一モータ188、第二モータ191に出力されるモータ出力値を算出する(S1103)。   When it is determined in the external force calculation unit 217 that the detected change in position is a change in position generated by operating the second link 184 and the third link 185 (Yes in S1102), the drive control unit 211. Calculates the motor output value output to the first motor 188 and the second motor 191 so that the second link 184 and the third link 185 move based on the detected change in position (S1103).

この時、駆動制御部211は、検知された位置の変化量に基づいて駆動部10、11を駆動させるために、電流値と駆動部10、11のトルク定数を乗算した値(モータ出力値)を回転駆動部212に出力する。回転駆動部212は、モータ出力値に基づいて、駆動部10、11を駆動させる(S1104)。   At this time, the drive control unit 211 multiplies the current value and the torque constant of the drive units 10 and 11 (motor output value) in order to drive the drive units 10 and 11 based on the detected change in position. Is output to the rotation drive unit 212. The rotation drive unit 212 drives the drive units 10 and 11 based on the motor output value (S1104).

駆動部10、11がモータ出力値に基づいて回転駆動することによって、第二リンク184、第三リンク185は移動される。   The second link 184 and the third link 185 are moved by the drive units 10 and 11 being driven to rotate based on the motor output value.

以上説明したように、本実施形態においては、第二リンク184、第三リンク185の移動状況に応じて、ユーザが移動させようとする方向にアシスト制御を行うため、マニピュレータ装置1の操作性を向上させることが出来る。従って、マニピュレータ装置1の質量や、第一モータ188、第二モータ191のトルクが大きく、ユーザが意図する操作が行いにくい場合でも、ユーザが操作しようとする方向に第二リンク184、第三リンク185が移動するようにアシスト制御を行う。そのため、ユーザは第二リンク184、第三リンク185を操作する際に、小さな力を加えるだけで、マニピュレータ装置1を動かすことが出来る。   As described above, in the present embodiment, the operability of the manipulator device 1 is improved because the assist control is performed in the direction in which the user tries to move according to the movement status of the second link 184 and the third link 185. Can be improved. Therefore, even when the mass of the manipulator device 1 and the torque of the first motor 188 and the second motor 191 are large and the operation intended by the user is difficult, the second link 184 and the third link are directed in the direction in which the user intends to operate. Assist control is performed so that 185 moves. Therefore, the user can move the manipulator device 1 only by applying a small force when operating the second link 184 and the third link 185.

また、ユーザの操作によっては、第二リンク184、第三リンク185が突然移動する等して、ユーザが意図しない方向に、マニピュレータ装置1が移動することも考えられる。これに対して、本実施形態に係る制御装置2においては、図12に示すように、第二リンク184、第三リンク185を移動させるために必要な移動力目標値を設定し、外力が不感帯を持つように設定する。さらに、予めマニピュレータ装置1が移動可能な位置の範囲や移動速度の設定値である位置速度指令値を設定する。   Further, depending on the user's operation, the manipulator device 1 may be moved in a direction not intended by the user due to sudden movement of the second link 184 and the third link 185. On the other hand, in the control device 2 according to the present embodiment, as shown in FIG. 12, the moving force target value necessary for moving the second link 184 and the third link 185 is set, and the external force is a dead zone. Set to have Furthermore, a position speed command value, which is a set value of a range of moving positions and a moving speed of the manipulator device 1, is set in advance.

このような移動力目標値、位置速度指令値を設定してマニピュレータ装置1の動作を制御する処理について以下、図16を参照して説明する。位置速度指令生成部222は、制御装置2に入力されたマニピュレータ装置1が移動可能な範囲や移動速度の設定値の情報に基づいて、位置速度指令値を設定する(S1301)。さらに、移動力目標生成部221は、制御装置2に入力された第二リンク184、第三リンク185を移動させるために必要な移動力目標値の情報に基づいて、移動力目標値を設定する(S1302)。尚、位置速度指令値または移動力目標値は何れか一つを設定する構成であってもよい。   Processing for controlling the operation of the manipulator device 1 by setting the moving force target value and the position / speed command value will be described below with reference to FIG. The position / speed command generation unit 222 sets the position / speed command value based on the range of movement of the manipulator device 1 input to the control device 2 and information on the set value of the moving speed (S1301). Further, the moving force target generator 221 sets the moving force target value based on the information on the moving force target value necessary for moving the second link 184 and the third link 185 input to the control device 2. (S1302). The position / speed command value or the moving force target value may be set to any one.

位置変化量検知部219において位置の変化量が検知される(S1303)と、外力算出部217は、第二リンク184、第三リンク185が移動されることで発生した位置の変化量であるか否か判定する(S1304)。   When the position change amount detection unit 219 detects a position change amount (S1303), is the external force calculation unit 217 a position change amount generated by moving the second link 184 and the third link 185? It is determined whether or not (S1304).

検知された位置の変化量が、外力算出部217において、第二リンク184、第三リンク185が操作されて発生した位置の変化ではないと判定された場合(S1304/No)、駆動制御部211は、本処理を終了させる。外力算出部217は、図11と同様に、例えば、マニピュレータ装置1がベース199ごと動かされて生じた位置の変化量については、第二リンク184、第三リンク185が操作されて発生した位置の変化ではないと判定する。   When it is determined in the external force calculation unit 217 that the detected change in position is not a change in position generated by operating the second link 184 and the third link 185 (S1304 / No), the drive control unit 211. Terminates this process. As in FIG. 11, the external force calculation unit 217, for example, regarding the amount of change in position that occurs when the manipulator device 1 is moved together with the base 199, is the position generated by the operation of the second link 184 and the third link 185. Judge that it is not a change.

検知された位置の変化量が、外力算出部217において、第二リンク184、第三リンク185が操作されて発生した位置の変化量であると判定された場合(S1304/Yes)、位置情報取得部216は、駆動部10、11の位置に基づいて、第二リンク184、第三リンク185の位置情報を出力する。   When the detected amount of change in position is determined by the external force calculation unit 217 to be the amount of change in position generated by operating the second link 184 and the third link 185 (S1304 / Yes), acquisition of position information The unit 216 outputs position information of the second link 184 and the third link 185 based on the positions of the driving units 10 and 11.

移動力目標生成部221は、位置情報及び外力算出部217において検知された位置の変化量に基づいて、S1302で設定した移動力目標値と外力の推定値とを比較する。そして、この比較結果に基づいて、移動力目標情報を生成し、駆動制御部211に送信する(S1305、移動力目標値誤差検知)。   The moving force target generator 221 compares the moving force target value set in S1302 with the estimated value of the external force based on the position information and the position change detected by the external force calculator 217. Based on the comparison result, the moving force target information is generated and transmitted to the drive control unit 211 (S1305, moving force target value error detection).

駆動制御部211は、移動力目標情報に基づいて第二リンク184、第三リンク185が移動力で移動するように、駆動部10、11を制御する。また、位置速度指令生成部222は、S1601にて設定した位置速度指令値とマニピュレータ装置1の移動情報及び移動速度とを比較する。そして、この比較結果に基づいて、位置速度指令情報を生成し、駆動制御部211に送信する(S1306)。   The drive control unit 211 controls the drive units 10 and 11 so that the second link 184 and the third link 185 move with the moving force based on the moving force target information. In addition, the position / speed command generation unit 222 compares the position / speed command value set in S1601 with the movement information and movement speed of the manipulator device 1. And based on this comparison result, position speed command information is produced | generated and transmitted to the drive control part 211 (S1306).

駆動制御部211は、推定された外力と同じ方向にアシスト制御が行われるように第一モータ188、第二モータ191に出力されるモータ出力値を回転駆動部212に出力する(S1307)。この時、検知された位置の変化量に基づいて駆動部10、11を駆動させるために、電流値と駆動部10、11のトルク定数を乗算した値と、S1305で生成された新たな移動力目標情報とS1306で生成された位置・速度を指定する情報とを参照して算出される。回転駆動部212は、モータ出力値に基づいて駆動部10、11を駆動させる(S1308)。   The drive control unit 211 outputs the motor output values output to the first motor 188 and the second motor 191 to the rotation drive unit 212 so that assist control is performed in the same direction as the estimated external force (S1307). At this time, in order to drive the drive units 10 and 11 based on the detected amount of change in position, a value obtained by multiplying the current value and the torque constant of the drive units 10 and 11 and the new moving force generated in S1305. It is calculated with reference to the target information and information specifying the position / velocity generated in S1306. The rotation drive unit 212 drives the drive units 10 and 11 based on the motor output value (S1308).

駆動部10、11がモータ出力値に基づいて回転駆動することによって、第二リンク184、第三リンク185は、S1305で生成された新たな移動力目標情報とS1306で生成された位置・速度を指定する情報に基づいて移動される。   When the drive units 10 and 11 are rotationally driven based on the motor output value, the second link 184 and the third link 185 have the new movement force target information generated in S1305 and the position and speed generated in S1306. Moved based on specified information.

以上説明したように、制御装置2に入力される移動力目標値及び位置速度設定値との誤差をフィードバックし、第二リンク184、第三リンク185を移動させるために、駆動部10、11の回転駆動のアシスト制御を実行する。このようにすることで、マニピュレータ装置1が突然動作することや、意図しない位置に移動することを抑制できる。また、移動力目標値を設定する範囲によっては、マニピュレータ装置1を所定の移動力を加えることで移動させることが可能である。   As described above, in order to feed back an error between the moving force target value and the position speed setting value input to the control device 2 and move the second link 184 and the third link 185, the driving units 10 and 11 Rotation drive assist control is executed. By doing in this way, it can control that manipulator device 1 operates suddenly, or moves to an unintended position. Further, depending on the range in which the moving force target value is set, the manipulator device 1 can be moved by applying a predetermined moving force.

以上説明したアシスト制御の処理においては、外力と同じ方向のベクトルにマニピュレータ装置1を直接教示する場合を例として説明を行った。これらの処理においては、推定された外力と逆方向(逆ベクトル)にマニピュレータ装置1が移動するように機能を切り替える制御を行うことも可能である。尚、マニピュレータ装置1がアシスト制御される方向を切り替える制御は、スイッチ28をON/OFF操作して切り替える構成であってもよい。また、推定された外力が所定以上である場合や回転速度を検知した場合に、アシスト制御を実行しない構成であってもよい。   In the assist control process described above, the case where the manipulator device 1 is directly taught to a vector in the same direction as the external force has been described as an example. In these processes, it is also possible to perform control to switch functions so that the manipulator device 1 moves in a direction opposite to the estimated external force (reverse vector). Note that the control for switching the direction in which the manipulator device 1 is assist-controlled may be configured to be switched by operating the switch 28 ON / OFF. Further, when the estimated external force is greater than or equal to a predetermined value or when the rotational speed is detected, a configuration in which the assist control is not executed may be used.

尚、マニピュレータ装置1を軽量化または小型化するために、第一関節軸エンコーダ173、第二関節軸エンコーダ174の代わりに、第一ワイヤ186の伸縮状況に基づいて第二リンク184、第三リンク185の移動量を検知してもよい。図14は、本実施形態に係る第一ワイヤ186に取り付けられるひずみゲージ186Bを示す図である。   In order to reduce the weight or size of the manipulator device 1, instead of the first joint axis encoder 173 and the second joint axis encoder 174, the second link 184 and the third link are based on the expansion / contraction state of the first wire 186. A movement amount of 185 may be detected. FIG. 14 is a view showing a strain gauge 186B attached to the first wire 186 according to the present embodiment.

図14に示すように、ひずみゲージ186Bは、第一ワイヤ186の直線部分と接続されたプレート186Aの上に設置されており、第一ワイヤ186の伸縮を電気的に検知することが出来る。従って、ひずみゲージ186Bは、ワイヤの伸縮量を検知する伸縮検知部として機能する。マニピュレータ装置1のユーザが、第二リンク184、第三リンク185を移動させた場合に、制御装置2とひずみゲージ186Bとに接続されている電気回路および伝達経路によって第一ワイヤ186の伸縮量が、外力算出部217に伝達される。   As shown in FIG. 14, the strain gauge 186 </ b> B is installed on a plate 186 </ b> A connected to the straight portion of the first wire 186, and can electrically detect expansion and contraction of the first wire 186. Accordingly, the strain gauge 186B functions as an expansion / contraction detection unit that detects the expansion / contraction amount of the wire. When the user of the manipulator device 1 moves the second link 184 and the third link 185, the amount of expansion and contraction of the first wire 186 depends on the electrical circuit and the transmission path connected to the control device 2 and the strain gauge 186B. Then, it is transmitted to the external force calculation unit 217.

外力算出部217は、ひずみゲージ186Bから伝達された第一ワイヤ186の伸縮量に基づいて、マニピュレータ装置1に加えられた外力の値を推定する。尚、第二ワイヤ187についても同様の構成にすることによって、第二ワイヤ187の伸縮量を外力算出部217に伝達することが出来る。   The external force calculation unit 217 estimates the value of the external force applied to the manipulator device 1 based on the amount of expansion / contraction of the first wire 186 transmitted from the strain gauge 186B. The second wire 187 can be transmitted to the external force calculation unit 217 by the same configuration as the second wire 187.

また、マニピュレータ装置1を軽量化または小型化するために、第一関節軸エンコーダ173、第二関節軸エンコーダ174の代わりに、第一ワイヤ186の張力調整部135によって第二リンク184、第三リンク185の移動量を検知することも出来る。図15は、本実施形態に係る第一ワイヤ186に取り付けられる張力調整部135の構造を示す図である。   Further, in order to reduce the weight or size of the manipulator device 1, instead of the first joint axis encoder 173 and the second joint axis encoder 174, the tension adjusting unit 135 of the first wire 186 allows the second link 184 and the third link. The amount of movement of 185 can also be detected. FIG. 15 is a diagram illustrating a structure of the tension adjusting unit 135 attached to the first wire 186 according to the present embodiment.

図15に示すように、張力調整部135は、張力検知部131、アイドラプーリ132、133、134を含む。張力検知部131は、図14で説明したひずみゲージ186Bを含んで構成され、第二リンク184、第三リンク185が移動させられて第一ワイヤ186に生じる張力の変化を検知する。また、アイドラプーリ132、133、134は、第一ワイヤ186を這いまわすように配置される。   As shown in FIG. 15, the tension adjustment unit 135 includes a tension detection unit 131 and idler pulleys 132, 133, and 134. The tension detector 131 includes the strain gauge 186 </ b> B described with reference to FIG. 14, and detects a change in tension generated in the first wire 186 when the second link 184 and the third link 185 are moved. The idler pulleys 132, 133, and 134 are arranged so as to twist the first wire 186.

外力算出部217は、張力検知部131から伝達された第一ワイヤ186の伸縮量に基づいて、マニピュレータ装置1に加えられた外力の値を推定する。尚、第二ワイヤ187についても同様の構成にすることによって、第二ワイヤ187の伸縮量を外力算出部217に伝達することが出来る。   The external force calculation unit 217 estimates the value of the external force applied to the manipulator device 1 based on the amount of expansion / contraction of the first wire 186 transmitted from the tension detection unit 131. The second wire 187 can be transmitted to the external force calculation unit 217 by the same configuration as the second wire 187.

さらに、第二リンク184、第三リンク185の移動方向を検知するために、図16及び図17に示すような移動方向検知部材500を用いてもよい。図16は、移動方向検知部材500に用いられる回転ダンパ501の構成を示す図、図17は、本実施形態に係る移動方向検知部材500である。   Furthermore, in order to detect the moving direction of the second link 184 and the third link 185, a moving direction detecting member 500 as shown in FIGS. 16 and 17 may be used. FIG. 16 is a diagram showing a configuration of a rotary damper 501 used for the movement direction detection member 500, and FIG. 17 shows the movement direction detection member 500 according to the present embodiment.

図16に示すように、回転ダンパ501は、入力軸501A、出力軸501B、シャッタ部501Cを含む。シャッタ部501Cは、出力軸501Bに付加され構成されている。また、図17に示すように、移動方向検知部材500は、第二リンク184、第三リンク185にそれぞれ装着されており、回転ダンパ501、ブラケット502、フォトインタラプタ503、スプリングプランジャ504を含む。   As shown in FIG. 16, the rotary damper 501 includes an input shaft 501A, an output shaft 501B, and a shutter unit 501C. The shutter unit 501C is configured to be added to the output shaft 501B. As shown in FIG. 17, the moving direction detection member 500 is attached to the second link 184 and the third link 185, and includes a rotary damper 501, a bracket 502, a photo interrupter 503, and a spring plunger 504.

回転ダンパ501は、第一従動プーリ195、第二従動プーリ198にそれぞれ装着されている。移動方向検知部材500は、ブラケット502によって第二リンク184、第三リンク185に固定されている。また、フォトインタラプタ503およびスプリングプランジャ504はブラケット502上に配置されている。   The rotary damper 501 is mounted on the first driven pulley 195 and the second driven pulley 198, respectively. The moving direction detection member 500 is fixed to the second link 184 and the third link 185 by a bracket 502. Further, the photo interrupter 503 and the spring plunger 504 are disposed on the bracket 502.

フォトインタラプタ503は、対向する発光部と受光部とから構成され、発光部から照射される光を受光部で検出する際の検出状況に応じて物体の有無あるいは位置を判定するセンサである。スプリングプランジャ504は、対向する弾性体によってシャッタ部501Cを保持する保持部材である。   The photo interrupter 503 is a sensor that includes a light emitting unit and a light receiving unit that are opposed to each other, and determines the presence or absence or position of an object according to a detection state when light emitted from the light emitting unit is detected by the light receiving unit. The spring plunger 504 is a holding member that holds the shutter portion 501C by an opposing elastic body.

図16に示す例においては、2つのフォトインタラプタ503がどちらもOFFである中立位置を表している。この状態からマニピュレータ装置1が操作された場合、入力軸501Aの回転が出力軸501Bに伝わり、シャッタ部501Cが回転する際の正または負の回転方向に応じて2つのフォトインタラプタ503のうち、いずれかをONにする。   In the example shown in FIG. 16, the neutral position where the two photo interrupters 503 are both OFF is shown. When the manipulator device 1 is operated from this state, the rotation of the input shaft 501A is transmitted to the output shaft 501B, and one of the two photo interrupters 503 is selected depending on the positive or negative rotation direction when the shutter unit 501C rotates. Turn on.

出力軸501Bの位相は、シャッタ部501Cが両側をスプリングプランジャ504によって押圧されているため、フォトインタラプタ503の検知位置からシャッタ部501Cが抜去されない範囲で動作する。入力軸501Aの回転が停止すると、弾性体の作用で出力軸501Bは中立位置に復帰し、次回入力軸501Aが回転するまで停止する。   The phase of the output shaft 501B operates within a range in which the shutter portion 501C is not removed from the detection position of the photo interrupter 503 because the shutter portion 501C is pressed on both sides by the spring plunger 504. When the rotation of the input shaft 501A stops, the output shaft 501B returns to the neutral position by the action of the elastic body, and stops until the next time the input shaft 501A rotates.

このように、従動プーリに伝達される駆動力の伝達方向に応じて制御されるフォトインタラプタ503のON信号に基づいてアシスト制御を開始することによって、簡単な回路構成およびアルゴリズムを実装したマニピュレータ装置1の制御装置2を実現することが出来る。以上説明したように、移動方向検知部材500は、従動プーリに伝達される駆動力の伝達方向に応じて第二リンク184及び第三リンク185の移動方向をそれぞれ検知する移動方向検知部として機能する。   In this way, the manipulator device 1 that implements a simple circuit configuration and algorithm by starting the assist control based on the ON signal of the photo interrupter 503 controlled according to the transmission direction of the driving force transmitted to the driven pulley. The control device 2 can be realized. As described above, the movement direction detection member 500 functions as a movement direction detection unit that detects the movement directions of the second link 184 and the third link 185 according to the transmission direction of the driving force transmitted to the driven pulley. .

尚、入力軸501Aと接続することが可能であれば、例えば、第一駆動プーリ194、第二駆動プーリ196や第一モータ減速装置190、第二モータ減速装置193もしくはモータ軸等に移動方向検知部材500を装着する構成であってもよい。   If it can be connected to the input shaft 501A, for example, the first drive pulley 194, the second drive pulley 196, the first motor speed reducer 190, the second motor speed reducer 193, or the motor shaft can be detected. The structure which mounts the member 500 may be sufficient.

また、移動方向を検知するために、マグネット式もしくはコイルスプリング式もしくはフリクション式のトルクリミッタを移動方向検知部材500として使用してもよい。さらに、ストローク型ダンパを回転ダンパ501の代わりに使用することも出来る。ストローク型ダンパを用いる場合には、マニピュレータ装置1における回転部材の他に、直線運動を行う部材にも適用することができるため、例えば、第一ワイヤ186、第二ワイヤ187や張力調整部135に対して適用することが可能である。   Further, a magnet type, coil spring type or friction type torque limiter may be used as the moving direction detecting member 500 in order to detect the moving direction. Further, a stroke type damper can be used instead of the rotary damper 501. In the case where the stroke type damper is used, it can be applied to a member that performs linear motion in addition to the rotating member in the manipulator device 1. For example, the first wire 186, the second wire 187, and the tension adjustment unit 135 can be used. It is possible to apply to.

尚、S1101及びS1303の処理においては、マニピュレータ装置1が停止状態から移動されたタイミングにおいて位置の変化量を検知してもよい。このようにすると、マニピュレータ装置1が動き出す際にアシスト制御を行うことが出来るため、ユーザが第二リンク184、第三リンク185を操作するために必要な力を小さくすることが出来る。   In the processing of S1101 and S1303, the amount of change in position may be detected at the timing when the manipulator device 1 is moved from the stopped state. If it does in this way, since assist control can be performed when the manipulator apparatus 1 starts moving, the force required for the user to operate the second link 184 and the third link 185 can be reduced.

1 マニピュレータ装置
10、11 駆動部
21 CPU
22 RAM
23 ROM
24 エンジン
25 HDD
26 I/F
27 センサ
28 スイッチ
29 バス
135 張力調整部
171 第一関節トルクリミッタ
172 第二関節トルクリミッタ
173 第一関節軸エンコーダ
174 第二関節軸エンコーダ
181 第一関節
182 第二関節
183 第一リンク
184 第二リンク
185 第三リンク
186 第一ワイヤ
187 第二ワイヤ
188 第一モータ
189 第一モータエンコーダ
190 第一モータ減速装置
191 第二モータ
192 第二モータエンコーダ
193 第二モータ減速装置
194 第一駆動プーリ
195 第一従動プーリ
196 第二駆動プーリ
197 第二中継プーリ
198 第二従動プーリ
199 ベース
186B ひずみゲージ
201 回転駆動制御部
211 駆動制御部
212 回転駆動部
214 回転検知部
215 FB取得部
216 位置情報取得部
217 外力算出部
218 外力検知部
219 位置変化量検知部
221 移動力目標生成部
222 位置速度指令生成部
500 移動方向検知部
DESCRIPTION OF SYMBOLS 1 Manipulator apparatus 10, 11 Drive part 21 CPU
22 RAM
23 ROM
24 engine 25 HDD
26 I / F
27 sensor 28 switch 29 bus 135 tension adjusting unit 171 first joint torque limiter 172 second joint torque limiter 173 first joint axis encoder 174 second joint axis encoder 181 first joint 182 second joint 183 first link 184 second link 185 3rd link 186 1st wire 187 2nd wire 188 1st motor 189 1st motor encoder 190 1st motor speed reducer 191 2nd motor 192 2nd motor encoder 193 2nd motor speed reducer 194 1st drive pulley 195 1st Driven pulley 196 Second drive pulley 197 Second relay pulley 198 Second driven pulley 199 Base 186B Strain gauge 201 Rotation drive control unit 211 Drive control unit 212 Rotation drive unit 214 Rotation detection unit 215 FB acquisition unit 216 Position information Information acquisition unit 217 External force calculation unit 218 External force detection unit 219 Position change amount detection unit 221 Movement force target generation unit 222 Position velocity command generation unit 500 Movement direction detection unit

国際公開第2013/027250号公報International Publication No. 2013/027250

Claims (10)

連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御装置であって、
前記交差箇所における前記移動部の移動状況の変化に基づいて前記移動部に加わる外力の推定値を算出する外力算出部と、
前記移動部を移動させるための駆動部を制御する駆動制御部と、
前記交差箇所において前記駆動部の動力に従動して駆動する従動部と、
前記駆動部の動力を前記従動部に伝達する駆動伝達部と、
を含み、
前記駆動制御部は、
前記移動状況が変化したタイミングに算出された前記外力の推定値に基づいて前記駆動部を制御し、
前記従動部は、
前記外力の推定値に基づいて制御される前記駆動部の動力に従動して前記移動部が移動するように駆動することを特徴とするマニピュレータ装置の制御装置。
A control device for a manipulator device in which a crossing point of a plurality of connected moving parts and a crossing point of a support part that supports the moving part and the moving part are driven, respectively.
An external force calculation unit that calculates an estimated value of the external force applied to the moving unit based on a change in the movement status of the moving unit at the intersection,
A drive control unit that controls a drive unit for moving the moving unit;
A driven portion that is driven and driven by the power of the drive portion at the intersection,
A drive transmission unit that transmits power of the drive unit to the driven unit;
Including
The drive control unit
Controlling the drive unit based on the estimated value of the external force calculated at the timing when the movement state changes,
The follower is
A control device for a manipulator device, wherein the moving unit is driven to move following the power of the driving unit controlled based on the estimated value of the external force.
前記駆動制御部は、
前記移動状況が変化したタイミングに前記移動部が移動させられた方向に前記移動部が移動するように前記駆動部を制御することを特徴とする請求項1に記載のマニピュレータ装置の制御装置。
The drive control unit
2. The control device for a manipulator device according to claim 1, wherein the driving unit is controlled such that the moving unit moves in a direction in which the moving unit is moved at a timing when the moving state changes.
前記駆動制御部は、
前記移動部が停止状態から移動させられたタイミングに前記移動部が移動させられた方向に前記移動部が移動するように前記駆動部を制御することを特徴とする請求項1または請求項2に記載のマニピュレータ装置の制御装置。
The drive control unit
The drive unit is controlled so that the moving unit moves in a direction in which the moving unit is moved at a timing when the moving unit is moved from a stopped state. The control device of the manipulator device described.
前記駆動伝達部の伸縮量を検知する伸縮検知部を含み、
前記外力算出部は、
前記駆動伝達部の伸縮状況に基づいて前記交差箇所における前記移動状況の変化を検知し、検知した前記移動状況の変化に基づいて前記推定値を算出することを特徴とする請求項1ないし請求項3いずれか一項に記載のマニピュレータ装置の制御装置。
Including an expansion / contraction detection unit for detecting an expansion / contraction amount of the drive transmission unit;
The external force calculator is
The change of the movement condition in the intersection is detected based on the expansion / contraction state of the drive transmission unit, and the estimated value is calculated based on the detected change of the movement condition. The control device of the manipulator device according to any one of 3.
前記移動部の移動量を光学的に検知する位置変化量検知部を含み、
前記外力算出部は、
前記交差箇所における前記移動状況の光学的な変化に基づいて前記推定値を算出することを特徴とする請求項1ないし請求項3いずれか一項に記載のマニピュレータ装置の制御装置。
A position change amount detection unit that optically detects a movement amount of the moving unit;
The external force calculator is
The control device for a manipulator device according to any one of claims 1 to 3, wherein the estimated value is calculated based on an optical change in the movement state at the intersection.
前記駆動伝達部の伸縮量を検知する伸縮検知部を含み、
前記外力算出部は、
検知された前記伸縮量に基づいて前記交差箇所における前記移動状況の変化を検知し、検知した前記移動状況の変化に基づいて前記推定値を算出することを特徴とする請求項1ないし請求項3いずれか一項に記載のマニピュレータ装置の制御装置。
Including an expansion / contraction detection unit for detecting an expansion / contraction amount of the drive transmission unit;
The external force calculator is
4. The estimated value is calculated on the basis of the detected change in the movement state, and a change in the movement state at the intersection is detected based on the detected amount of expansion / contraction. The control device for the manipulator device according to any one of the preceding claims.
前記交差箇所における前記移動部の移動方向を検知する移動方向検知部を含み、
前記移動方向検知部は、
前記従動部に前記駆動部の動力を伝達する際の伝達方向に基づいて前記移動部の移動方向を検知することを特徴とする請求項1ないし請求項3いずれか一項に記載のマニピュレータ装置の制御装置。
Including a moving direction detection unit that detects a moving direction of the moving unit at the intersection,
The moving direction detector is
The manipulator device according to any one of claims 1 to 3, wherein a moving direction of the moving unit is detected based on a transmission direction when the power of the driving unit is transmitted to the driven unit. Control device.
前記交差箇所における前記移動部の移動方向を検知する移動方向検知部を含み、
前記移動方向検知部は、
前記伸縮量に基づいて前記移動部の移動方向を検知することを特徴とする請求項6に記載のマニピュレータ装置の制御装置。
Including a moving direction detection unit that detects a moving direction of the moving unit at the intersection,
The moving direction detector is
The control device for a manipulator device according to claim 6, wherein the moving direction of the moving unit is detected based on the amount of expansion / contraction.
連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御方法であって、
前記交差箇所における前記移動部の移動状況の変化に基づいて前記移動部に加わる外力の推定値を算出し、
前記移動部を移動させるための駆動部を前記移動状況が変化したタイミングに算出された前記外力の推定値に基づいて制御し、
前記交差箇所において前記駆動部の動力に従動して駆動する従動部に、前記駆動部の動力を伝達し、
前記従動部を、前記推定値に基づいて制御される前記駆動部の動力に従動して前記移動部が移動するように駆動させることを特徴とするマニピュレータ装置の制御方法。
A control method of a manipulator device in which a crossing point of a plurality of connected moving parts and a crossing point of a support part supporting the moving part and the moving part are driven, respectively.
Calculate an estimated value of the external force applied to the moving part based on a change in the moving state of the moving part at the intersection,
Controlling the driving unit for moving the moving unit based on the estimated value of the external force calculated at the timing when the moving state has changed,
Transmitting the power of the drive unit to a driven unit driven by the power of the drive unit at the intersection,
A method for controlling a manipulator device, wherein the driven unit is driven so that the moving unit moves following the power of the driving unit controlled based on the estimated value.
連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御プログラムであって、
前記交差箇所における前記移動部の移動状況の変化に基づいて前記移動部に加わる外力の推定値を算出するステップと、
前記移動部を移動させるための駆動部を前記移動状況が変化したタイミングに算出された前記外力の推定値に基づいて制御するステップと、
前記交差箇所において前記駆動部の動力に従動して駆動する従動部に、前記駆動部の動力を伝達するステップと、
前記従動部を、前記推定値に基づいて制御される前記駆動部の動力に従動して前記移動部が移動するように駆動させるステップと、
を実行させることを特徴とするマニピュレータ装置の制御プログラム。
A control program for a manipulator device that drives a crossing point of a plurality of connected moving parts and a crossing point of a supporting part that supports the moving part and the moving part, respectively.
Calculating an estimated value of an external force applied to the moving unit based on a change in the movement status of the moving unit at the intersection;
Controlling a driving unit for moving the moving unit based on the estimated value of the external force calculated at a timing when the moving state changes;
Transmitting the power of the drive unit to a driven unit that is driven and driven by the power of the drive unit at the intersection;
Driving the driven unit so that the moving unit moves following the power of the driving unit controlled based on the estimated value;
A control program for a manipulator device, characterized in that
JP2016099918A 2016-05-18 2016-05-18 Control device of manipulator device, control method of the manipulator device, and control program of the manipulator device Pending JP2017205835A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2016099918A JP2017205835A (en) 2016-05-18 2016-05-18 Control device of manipulator device, control method of the manipulator device, and control program of the manipulator device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2016099918A JP2017205835A (en) 2016-05-18 2016-05-18 Control device of manipulator device, control method of the manipulator device, and control program of the manipulator device

Publications (1)

Publication Number Publication Date
JP2017205835A true JP2017205835A (en) 2017-11-24

Family

ID=60415209

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2016099918A Pending JP2017205835A (en) 2016-05-18 2016-05-18 Control device of manipulator device, control method of the manipulator device, and control program of the manipulator device

Country Status (1)

Country Link
JP (1) JP2017205835A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110299062A (en) * 2018-03-21 2019-10-01 北京猎户星空科技有限公司 Mechanical arm teaching system, method, apparatus, medium, controller and mechanical arm

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110299062A (en) * 2018-03-21 2019-10-01 北京猎户星空科技有限公司 Mechanical arm teaching system, method, apparatus, medium, controller and mechanical arm

Similar Documents

Publication Publication Date Title
JP5895628B2 (en) ROBOT CONTROL METHOD, ROBOT CONTROL DEVICE, AND ROBOT CONTROL SYSTEM
JP2016013613A (en) Robot control method, robot device, program, recording medium, and method for manufacturing assembled component
JP5383911B2 (en) Robot controller
US20160263749A1 (en) Joint driving apparatus and robot apparatus
US20150217445A1 (en) Method For Programming An Industrial Robot and Industrial Robot
SE0301531D0 (en) A Control method for a robot
WO2015101088A1 (en) Intelligent control method, device and system for multi-joint mechanical arm
TWI651175B (en) Control device of robot arm and teaching system and method using the same
JP2011101938A (en) Robot and control device for the same
EP3083160A1 (en) Device for dynamic switching of robot control points
JP2017205835A (en) Control device of manipulator device, control method of the manipulator device, and control program of the manipulator device
JP2008528307A (en) 5-bar mechanism with dynamic balancing means and method for dynamically balancing a 5-bar mechanism
JP4842561B2 (en) Force controller device
WO2016162066A1 (en) An industrial robot and a method for lead-through programming of an industrial robot
JP2012051043A (en) Robot system or robot control device
JP5633268B2 (en) Robot control device
JP2017164833A (en) Control device of manipulator device, control method of manipulator device, and control program of manipulator device
US20210260757A1 (en) Dual mode free-drive of robot arm
JP2017202554A (en) Control device of manipulator device, control method of the manipulator device, and control program of the manipulator device
Nambi et al. Toward intuitive teleoperation of micro/nano-manipulators with piezoelectric stick-slip actuators
JP2017035756A (en) Robot control device suppressing vibration of tool tip end of robot having travel shaft
JP5961077B2 (en) Robot control apparatus and robot control method
JP2019126903A (en) Automatic control device and automatic control method
JP2017074668A (en) Controller for manipulator device, method for controlling manipulator device and control program for manipulator device
JP2020001099A (en) Control device, robot and robot system