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 PDFInfo
- 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
Links
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
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.
直接教示においては、マニピュレータ装置を小さな力で直接教示できるようにモータを駆動させるアシスト制御が行われる。マニピュレータ装置の駆動方法のひとつとして、マニピュレータ装置の関節部分から離れた位置に設置したモータを駆動させ、プーリとベル
トもしくはワイヤによって駆動力を伝達させてマニピュレータ装置を関節ごとに駆動させる方法がある（例えば、特許文献１参照）。
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).
しかし、特許文献１に開示された技術は、マニピュレータ装置を動作させるために、加えられた力（外力）を検知したことをトリガとして、アシスト制御が開始される。従って、直接教示を行うためにマニピュレータ装置を動かす際に、装置の重量や大きさによっては、大きな力を加える必要がある。 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.
以下、図面を参照して、本発明の実施形態を詳細に説明する。本実施形態においては、自由度が「２」である垂直多関節型のロボットアームを例として、マニピュレータ装置の関節にあたる駆動部での動作を補助するアシスト制御について説明する。 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.
図１は、本実施形態に係るマニピュレータ装置１の機械構成図である。図１に示すように、本実施形態に係るマニピュレータ装置１は、第一関節トルクリミッタ１７１、第二関節トルクリミッタ１７２、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４、第一関節１８１、第二関節１８２、第一リンク１８３、第二リンク１８４、第三リンク１８５、第一ワイヤ１８６、第二ワイヤ１８７、第一モータ１８８、第一モータエンコーダ１８９、第一モータ減速装置１９０、第二モータ１９１、第二モータエンコーダ１９２、第二モータ減速装置１９３、第一駆動プーリ１９４、第一従動プーリ１９５、第二駆動プーリ１９６、第二中継プーリ１９７、第二従動プーリ１９８、ベース１９９を含む。 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. .
第一リンク１８３、第二リンク１８４、第三リンク１８５は、その交差箇所を、夫々第一関節１８１、第二関節１８２を回転軸とした回転可能な機構によって支持され、第一モータ１８８、第二モータ１９１によって移動させられる移動部として機能する。また、ベース１９９は、第一リンク１８３、第二リンク１８４、第三リンク１８５を支持する支持部として機能する。第二関節１８２によって支持されていない方の第三リンク１８５の端部には、把持動作を行うピッキングハンドやエアーチャック等のリンク機構が備えられている。 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.
また、リンク機構と第三リンク１８５との接続部分には、マニピュレータ装置１が移動したことを検知する移動検知センサ１１２が含まれる構成であってもよい。この移動検知センサ１１２が含まれる場合、後述する外力検知部２１８と連動して機能する。 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.
尚、移動検知センサ１１２として、六軸力覚センサ等を用いる構成であってもよい。本実施形態に係るマニピュレータ装置１は、図４で説明する制御装置２が出力する制御信号に基づいて動作を制御される。 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.
まず、第一モータ１８８による第一リンク１８３の駆動態様について説明する。第一リンク１８３には駆動部である第一モータ１８８が取り付けられており、第一モータ１８８の回転動作を制御する機構としてエンコーダ１８９が同軸上に取り付けられている。また、第一リンク１８３には、第一モータ１８８と同軸上に第一モータ減速装置１９０が取り付けられ、第一モータ減速装置１９０に第一駆動プーリ１９４が取り付けられている。 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.
一方で、第一リンク１８３は第一関節１８１を回転軸として、第一従動プーリ１９５によって回動可能に支持されている。第一駆動プーリ１９４及び第一従動プーリ１９５には駆動伝達部である第一ワイヤ１８６が巻きつけられている。 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.
また、第一関節１８１には一定のトルクを保持可能にするために、トルクリミッタ１７１が取り付けられている。そのため、トルクリミッタ１７１は、第二リンク１８４が第一関節１８１を回転軸として回転駆動する際に負荷として作用する、落下防止ブレーキとしても機能する。 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.
尚、図１の構成では出力軸である第一関節１８１にトルクリミッタ１７１が取り付けられているが、第一モータ１８８にトルクリミッタあるいは電磁ブレーキが取り付けられる構成であってもよい。 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.
また、第一関節１８１にはさらに光学式の第一関節軸エンコーダ１７３が取り付けられている。第一関節軸エンコーダ１７３は、回転検知部として機能する。第一関節軸エンコーダ１７３は、ワイヤの伸び等を考慮して第一関節１８１に取り付けられる。尚、第一ワイヤ１８６を十分な張力で張り、ワイヤの伸びが発生しなければ、第一モータエンコーダ１８９と第一関節軸エンコーダ１７３とは線形性を有するため、第一関節軸エンコーダ１７３は不要である。 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.
以上説明したように、第一モータ１８８による駆動力は、第一駆動プーリ１９４から第一ワイヤ１８６に伝達され、第一ワイヤ１８６が第一関節１８１を水平回転軸として第一従動プーリ１９５を回転駆動させる。そして、第一従動プーリ１９５は、第一従動プーリ１９５に固定されている第二リンク１８４を、第一関節１８１を水平回転軸として垂直に回転駆動させる。 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.
次に第二モータ１９１による第一リンク１８３の駆動態様について説明する。第一リンク１８３には駆動部である第二モータ１９１が取り付けられており、第二モータ１９１の回転動作を制御する機構としてエンコーダ１９２が同軸上に取り付けられている。また、第一リンク１８３には、第二モータ１９１と同軸上に第二モータ減速装置１９３が取り付けられ、第二モータ減速装置１９３に第二駆動プーリ１９６が取り付けられている。 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.
一方で、第一リンク１８３は第一関節１８１を回転軸として、第二中継プーリ１９７によっても回転可能に支持されている。また、第二リンク１８４は第二関節１８２を回転軸として、第二従動プーリ１９８によって回転可能に支持されている。第二駆動プーリ１９６、第二中継プーリ１９７、第二従動プーリ１９８には、夫々駆動伝達部である第二ワイヤ１８７が巻きつけられている。 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.
また、第二関節１８２には一定のトルクを保持可能にするためにトルクリミッタ１７２が取り付けられている。そのため、トルクリミッタ１７２は、第三リンク１８５が第二関節１８２を回転軸として回転駆動する際に負荷として作用する、落下防止ブレーキとしても機能する。 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.
尚、図１の構成では出力軸である第二関節１８２にトルクリミッタ１７２が取り付けられているが、第二モータ１９１にトルクリミッタあるいは電磁ブレーキが取り付けられる構成であってもよい。 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.
また、第二関節１８２にはさらに光学式の第二関節軸エンコーダ１７４が取り付けられている。第二関節軸エンコーダ１７４は、回転検知部として機能する。第二関節軸エンコーダ１７４は、ワイヤの伸び等を考慮して第二関節１８２に取り付けられる。尚、第二ワイヤ１８７を十分な張力で張り、ワイヤの伸びが発生しなければ、第二モータエンコーダ１９２と第二関節軸エンコーダ１７４は線形性を有するため、第二関節軸エンコーダ１７４は不要である。 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.
以上説明したように、第二モータ１９１による駆動力は、第二駆動プーリ１９６から第二ワイヤ１８７に伝達され、第二ワイヤ１８７が第二関節１８２を水平回転軸として第二従動プーリ１９８を回転駆動させる。そして、第二従動プーリ１９８は、第二従動プーリ１９８に固定されている第三リンク１８５を、第二関節１８２を水平回転軸として垂直に回転駆動させる。 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.
次に、図２及び図３を参照して、本実施形態に係るワイヤ駆動部の構成について説明する。図２は、本実施形態に係るワイヤ駆動部の基礎構成図、図３は、本実施形態に係るワイヤ駆動部の基礎構成分解図である。 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.
図２に示すように、第一駆動プーリ１９４及び第一従動プーリ１９５には駆動伝達部として第一ワイヤ１８６が巻きつけられている。第一ワイヤ１８６は図２において時計回りの回転を伝達する１８６Ａ部と、反時計回りの回転を伝達する１８６Ｂ部とに分かれて巻き付けられている。 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.
また、第一駆動プーリ１９４は第一ワイヤ１８６の１８６Ａ部と１８６Ｂ部を夫々受ける１９４Ａ部と１９４Ｂ部とで構成されている。さらに、第一従動プーリ１９５は第一ワイヤ１８６の１８６Ａ部と１８６Ｂ部を夫々受ける１９５Ａ部と１９５Ｂ部とで構成されている。 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.
図３に示すように、第一ワイヤ１８６の端部には、固定部材２０１Ａおよび２０１Ｂが取り付けられている。この固定部材２０１Ａおよび２０１Ｂを第一駆動プーリの溝１９４Ａ、１９４Ｂに夫々組み込むことで第一ワイヤ１８６は第一駆動プーリ１９４に固定される。以上の構成は第二駆動プーリ１９６と、その関連部についても同様であるため、重複する説明を省略する。 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.
図４は、本実施形態に係るマニピュレータ装置１を制御する制御装置２のハードウェア構成を示すブロック図である。図４に示すように、本実施形態に係る制御装置２は、一般的なサーバやＰＣ（Ｐｅｒｓｏｎａｌ Ｃｏｍｐｕｔｅｒ）等の情報処理端末と同様の構成に加えて、モータ等の駆動機構を制御するエンジンを有する。即ち、本実施形態に係る制御装置２は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）２１、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）２２、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）２３、エンジン２４、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）２５及びＩ／Ｆ２６がバス２９を介して接続されている。Ｉ／Ｆ２６には、センサ２７やスイッチ２８が接続され、スイッチ２８は、マニピュレータ装置１の電源を制御するものである。 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.
また、センサ２７は、図１の移動検知センサ１１２を含み、マニピュレータ装置１の状態を検知するものである。さらに、Ｉ／Ｆ２６にはタブレット端末等の外部装置がネットワークを介して接続され、マニピュレータ装置１の動作に関する設定を入力することが出来る。 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.
ＣＰＵ２１は演算手段であり、マニピュレータ装置１全体の動作を制御する。ＲＡＭ２２は、情報の高速な読み書きが可能な揮発性の記憶媒体であり、ＣＰＵ２１が情報を処理する際の作業領域として用いられる。ＲＯＭ２３は、読み出し専用の不揮発性記憶媒体であり、ファームウェア等のプログラムが格納されている。エンジン２４は、マニピュレータ装置１において実際にモータ駆動を実行する機構である。 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.
ＨＤＤ２５は、情報の読み書きが可能な不揮発性の記憶媒体であり、ＯＳ（Ｏｐｅｒａｔｉｎｇ Ｓｙｓｔｅｍ）や各種の制御プログラム、アプリケーション・プログラム等が格納されている。Ｉ／Ｆ２６は、バス２９と各種のハードウェアやネットワーク等を接続し制御する。 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.
このようなハードウェア構成において、ＲＯＭ２３に格納されたプログラムや、ＨＤＤ２５若しくは図示しない光学ディスク等の記録媒体からＲＡＭ２２に読み出されたプログラムに従ってＣＰＵ２１が演算を行うことにより、ソフトウェア制御部が構成される。このようにして構成されたソフトウェア制御部と、ハードウェアとの組み合わせによって、本実施形態に係る制御装置２の機能を実現する機能ブロックが構成される。 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.
図５は、本実施形態に係るマニピュレータ装置１の動作機構を示す図である。図１に示したようなマニピュレータ装置１は、制御装置２によって動作を制御される。制御装置２はＰＣ等の演算機能を備えた端末であるため、制御装置２を操作して、マニピュレータ装置１の位置や速度をユーザが任意に設定することが出来る。また、外部装置から制御装置２に入力される命令に基づいてマニピュレータ装置１の動作を制御する構成であってもよい。 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.
第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４は、回転速度や回転位置及び回転方向を検知するためのマーカーが付加された円板を含み、そのマーカーを光学的に読み取ることによって検知信号を出力する。従って、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４は、移動方向検知部としても機能する。尚、検知信号の出力態様について詳細は後述する。 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.
第一モータ減速装置１９０、第二モータ減速装置１９３は、それぞれ、第一モータ１８８、第二モータ１９１の回転数を減少させ、高いトルクを得るための装置である。また、第一モータ１８８、第二モータ１９１の回転数とマニピュレータ装置１が必要とする回転数が異なる場合に第一モータ１８８、第二モータ１９１の回転数を調節する。 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.
第一モータ１８８、第二モータ１９１は、マニピュレータ装置１の動力装置であり、マニピュレータ装置１は第一モータ１８８、第二モータ１９１の回転駆動により発生した動力を第一ワイヤ１８６、第二ワイヤ１８７によって伝達させ、第一従動プーリ１９５、第二従動プーリ１９８を駆動させる。従って、第一モータ１８８、第二モータ１９１は第一従動プーリ１９５、第二従動プーリ１９８を回転駆動させる駆動部１０、１１として機能する。 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.
また、第一ワイヤ１８６、第二ワイヤ１８７は、第一モータ１８８、第二モータ１９１の動力を伝達する駆動伝達部として機能する。さらに、第一駆動プーリ１９４、第二駆動プーリ１９６は、第一モータ１８８、第二モータ１９１の動力に基づいて従動する従動部として機能する。 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.
図５は、本実施形態に係る制御装置２の機能構成を示すブロック図である。図４に示すように、制御装置２は、駆動制御部２１１、回転駆動部２１２、ＦＢ（フィードバック）取得部２１５、位置情報取得部２１６、外力算出部２１７を含む。尚、マニピュレータ装置１に移動検知センサ１１２が含まれる構成である場合、制御装置２は、図６に示すように、外力算出部２１７の内部機能として外力検知部２１８を含む。本実施形態にかかる制御装置２は、フィードバック制御部として機能し、マニピュレータ装置１は制御装置２から出力された信号によって動作を制御される。 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.
駆動制御部２１１は、Ｉ／Ｆを介して外部装置から入力される駆動部１０、１１の回転速度の目標値、ＦＢ取得部２１５から入力されるフィードバック信号、外力算出部２１７から入力される外力の推定値に基づき、駆動部１０、１１を回転させるための制御値を回転駆動部２１２に対して出力する。回転駆動部２１２は、駆動制御部２１１から入力される制御値に基づき、駆動部１０、１１を回転させるためのＰＷＭ信号を生成して出力する。 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.
駆動部１０、１１は、回転駆動部２１２から入力されるＰＷＭ信号に応じて回転する第一モータ１８８、第二モータ１９１である。この駆動部１０、１１の回転駆動によって、第一関節１８１、第二関節１８２を回転軸としてマニピュレータ装置１が移動される。駆動部１０、１１としては、ＤＣ（Ｄｉｒｅｃｔ Ｃｕｒｒｅｎｔ）モータを用いることが可能であり、インナーロータ型ＤＣモータであるブラシレスＤＣモータやブラシ付きＤＣモータを用いることが可能である。 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.
本実施形態に係る駆動部１０、１１は、回転速度や回転位置を検知するためのマーカーが付加された円板を回転させる。また、第一関節１８１、第二関節１８２が、駆動部１０、１１の回転駆動に従動して回転駆動することによって、回転速度や回転位置を検知するためのマーカーが付加された円盤を回転させる。 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. .
そして、第一モータエンコーダ１８９、第二モータエンコーダ１９２によって構成される回転検知部２１４は、そのマーカーを光学的に読み取ることによって検知信号を出力する。また、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４によって構成される位置変化量検知部２１９も、円盤に付加されたマーカーを光学的に読み取ることによって検知信号を外力算出部２１７に出力する。 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. .
具体的には、駆動部１０、１１の回転に伴って回転する円盤に付加されたマーカーが、回転検知部２１４が光学的に読み取る読み取り位置を通過する。これにより、回転検知部２１４による光学的な読み取り状態が変化し、回転検知部２１４がマーカーを検知して検知信号を出力する。尚、位置変化量検知部２１９においても同様に、第一関節１８１、第二関節１８２の回転に伴って回転する円盤に付加されたマーカーが、位置変化量検知部２１９が光学的に読み取る位置を通過する。 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.
尚、本実施形態に係る回転検知部２１４及び位置変化量検知部２１９は２組のセンサを含む。この２組のセンサは、上述した円板に付加されたマーカーの位相差がπ／２（ｒａｄ）となるように配置されている。これにより、本実施形態に係る回転検知部２１４及び位置変化量検知部２１９は、π／２（ｒａｄ）の位相差を有する２つの検知信号を出力する。 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).
ＦＢ取得部２１５は、回転検知部２１４の検知信号を取得し、単位時間あたりにマーカーが検知された回数に基づいて、駆動部１０、１１の回転位置を計算し、その計算結果から駆動部１０、１１の回転速度を算出する。算出された駆動部１０、１１の回転速度は、フィードバック値として駆動制御部２１１に入力される。また、ＦＢ取得部２１５は、上述したように、π／２（ｒａｄ）の位相差を有する２つの検知信号の位相差を利用して、駆動部１０、１１の回転方向を検知する。 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).
このようにして出力されるＦＢ取得部２１５のフィードバック値に基づき、駆動制御部２１１は、外部装置から入力される目標値と、ＦＢ取得部２１５から入力されるフィードバック値との差分に基づいて、駆動部１０、１１を回転させるための制御値を出力する。 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.
位置情報取得部２１６は、上述したような制御装置２におけるフィードバック制御のサイクルにおいて、駆動部１０、１１の回転状態を示す値である特徴量を取得する。図５において破線で示しているように、駆動部１０、１１の回転状態を示す値としては、ＦＢ取得部２１５が取得した回転検知部２１４の検知信号や、ＦＢ取得部２１５が算出した駆動部１０、１１の回転速度、外力算出部２１７が算出した第一関節１８１、第二関節１８２の回転位置を用いることができる。 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.
また駆動制御部２１１が回転駆動部２１２に入力する制御値を用いることができる。更に、回転駆動部２１２が出力するＰＷＭ信号を用いることができる。これらの値が、フィードバック制御において得られるフィードバック値である。位置情報取得部２１６は、取得したマニピュレータ装置１の位置情報を駆動制御部２１１に返信する。 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.
外力算出部２１７は、位置変化量検知部２１９の検知信号を取得し、単位時間あたりにマーカーが検知された回数に基づいて、第一関節１８１、第二関節１８２の回転位置を算出する。尚、この計算結果から、第一関節１８１、第二関節１８２の回転速度を算出する構成であってもよい。 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.
以上説明したように、外力算出部２１７は、第一従動プーリ１９５、第二従動プーリ１９８に設置された第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４から送信される信号に基づいて、マニピュレータ装置１に加えられている外力の推定値を算出する。 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.
マニピュレータ装置１のユーザがマニピュレータ装置１を動かそうとした場合、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４は、それぞれ第一関節１８１、第二関節１８２における位置の変化量を検知する。この時、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４において検知される位置の変化量とは、第一関節１８１と第二関節１８２夫々の関節軸に対して第一リンク１８３及び第二リンク１８４、第二リンク１８４と第三リンク１８５が夫々形成する角度の変化量のことを示す。 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.
外力算出部２１７は、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４が位置の変化量を検知したタイミングにおいて、その位置の変化量に基づいて、マニピュレータ装置１に加えられている外力の推定値を算出する。 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.
尚、マニピュレータ装置１の先端部分に移動検知センサ１１２が設置されている場合に、制御装置２が外力検知部２１８を含む構成であってもよい。外力検知部２１８は、移動検知センサ１１２によって第二リンク１８４、第三リンク１８５に力が加えられたこと（外力）を検知すると、第一モータ１８８、第二モータ１９１への入力トルクやマニピュレータ装置１の位置及び速度等の情報を取得する。そして取得した情報に基づいて外力を検知する。 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.
また、制御装置２に外力検知部２１８が含まれる構成である場合、外力算出部２１７が、外力検知部２１８で検知された外力の値に基づいてマニピュレータ装置１に加えられている外力の推定値を算出する構成であってもよい。 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.
尚、本実施形態に係るフィードバック制御においては、第一関節１８１、第二関節１８２を回転駆動させるために、駆動制御部２１１が、駆動部１０、１１の回転速度及び第一関節１８１、第二関節１８２の位置の目標値とフィードバック値とに基づいて制御値を出力する。即ち、回転速度に基づいた制御（以降、「速度制御」とする）が行われる。また、第一関節１８１、第二関節１８２の回転位置に基づいた制御（以降、「位置制御」とする）が行われる。第二リンク１８４、第三リンク１８５が移動されることで加わる外力と、装置自体の回転速度及び位置情報に基づいて、マニピュレータ装置１の動作を制御することが本実施形態に係る要旨の１つである。 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.
位置制御を行う場合、ＦＢ取得部２１５は、マーカーが検知された回数に基づいて駆動部１０、１１の回転位置を計算し、フィードバック値として駆動制御部２１１に入力する。また、駆動制御部２１１には、第一関節１８１、第二関節１８２の回転位置の目標値が入力される。これにより、駆動制御部２１１は、目標値とフィードバック値との差分に基づいて駆動部１０、１１の回転を制御するための制御値を生成して出力する。これにより、第一関節１８１、第二関節１８２の回転位置が、入力された目標値に保たれる。 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.
このように、駆動部１０、１１に従動して回転駆動する第一関節１８１と第二関節１８２は、制御装置２によってその回転駆動を制御される。 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.
図７は、本実施形態に係る制御装置２の特徴的な機能を実現する駆動制御部２１１の機能構成を示す図である。図７に示すように、駆動制御部２１１は、移動力目標生成部２２１、位置速度指令生成部２２２を含む。移動力目標生成部２２１は、制御装置２に入力されるマニピュレータ装置１を移動させる移動力を設定する情報に基づいて、第二リンク１８４、第三リンク１８５を移動させる時の移動力目標値の設定を行う。また、マニピュレータ装置１の位置や外力の変化から、移動力目標値の設定を変更して第一関節１８１、第二関節１８２を回転させるために駆動部１０、１１の駆動を制御するための移動力目標情報を生成する。 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.
位置速度指令生成部２２２は、制御装置２に入力されるマニピュレータ装置１の移動範囲や移動速度を設定する情報に基づいて、第二リンク１８４、第三リンク１８５を移動させる位置速度目標値の設定を行う。また、マニピュレータ装置１の位置や移動速度の変化と目標値との差分に基づいて、第一関節１８１、第二関節１８２を回転させるために駆動部１０、１１の駆動を制御しマニピュレータ装置１の位置・移動速度の設定を変更するための位置速度指令情報を生成する。 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.
図８（ａ）及び（ｂ）は、本実施形態に係るマニピュレータ装置１において発生する力について説明する図である。本実施形態においては、第二リンク１８４、第三リンク１８５が第一関節１８１、第二関節１８２で接続された２関節マニピュレータの平面運動と第三リンク１８５の自由末端における力Ｆとの関係から、第一関節トルクリミッタ１７１、第二関節トルクリミッタ１７２に加わるトルクを算出する。 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.
図８（ａ）は、本実施形態に係るマニピュレータ装置１のｘｙ平面での動作を模式的に示した図である。ここでは、第二関節１８２の第二関節トルクリミッタ１７２によって第二リンク１８４と連結されていない側の第三リンク１８５の末端が自由末端となる。実際のマニピュレータ装置１の動作においては、この自由末端には、移動検知センサ１１２が取り付けられ、外力の検知を行う構成であってもよい。 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.
図８（ｂ）は、図８（ａ）にて示したマニピュレータ装置１に発生する物理量を説明する図である。図８（ｂ）においては、夫々、Ｉｉ：慣性モーメント、Ｌｉ：リンクの長さ、ｍｉ：質量、θｉ：角度、τｉ：トルク、ｒｉ：関節中心と質量中心との距離、ｇ：重力加速度（ｙ軸負方向）を示している。また、本実施形態においては、自由度２のマニピュレータ装置１を例として説明するので、ｉ＝１，２とする。 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.
図８（ｂ）に示すように、マニピュレータ装置１がｘｙ平面で動作している時の運動方程式は、（数１）で与えられる。
数１において、ｑは一般化座標を示し、ｑは（数２）のようにあらわされる。
（数３）に示すように、本実施形態に係るマニピュレータ装置１に（数１）に示す運動方程式を適用すると、動作中に第一関節トルクリミッタ１７１、第二関節トルクリミッタ１７２に加わるトルクを推定することが出来る。
（数３）で推定したトルクは、マニピュレータ装置１の機械構成や減速装置、剛性、粘性摩擦等を考慮することで更に精度を高めることが出来る。また、数３で推定したトルクと、第三リンク１８５の自由末端における力Ｆとには、（数４）に示す関係がある。
また、本実施形態に係るマニピュレータ装置１のヤコビ行列は、
次に、図９を参照して本実施形態に係るマニピュレータ装置１を直接教示する際の動作について説明する。図９は、本実施形態に係るマニピュレータ装置１の動作を直接教示する際にユーザが行う動作を例示した図である。本実施形態に係るマニピュレータ装置１のように、マニピュレータ装置の関節部およびその駆動方式として、関節の回転軸と同じ軸にモータおよび減速装置を配置しているものがある。 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.
図９に示すように、マニピュレータ装置１を直接教示する際に、上述したような理由によってマニピュレータ装置１を移動させることが困難な場合がある。そこで、本実施形態に係るマニピュレータ装置１においては、外力が検知される前に、マニピュレータ装置１が教示される方向に働く力（移動量）を検知する。また、移動量が検知されたことをトリガとしてマニピュレータ装置１のアシスト制御を開始することにより、直接教示を行う方向にいち早く力が付与されるため、マニピュレータ装置１移動状況に応じて、操作性を向上させた直接教示を行うことが可能になる。 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.
このように、ユーザによるマニピュレータ装置１の移動を補助するために、駆動部１０、１１に対して行う制御のことを以後、「アシスト制御」と記載する。図９及び図１０に示すように、ユーザが直接教示する方向にマニピュレータ装置１のアシスト制御を行い、操作性を高めて直接教示を行うことが本発明の要旨の１つである。 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.
次に、図１１を参照して、本実施形態に係るマニピュレータ装置１のアシスト制御の流れについて説明する。図１１は、本実施形態に係るマニピュレータ装置１が移動される際にアシスト制御を行う処理の流れを示すフローチャートである。位置変化量検知部２１９において位置の変化量が検知される（Ｓ１１０１）と、外力算出部２１７は、第二リンク１８４、第三リンク１８５が移動されることで発生した位置の変化量であるか否か判定する（Ｓ１１０２）。 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).
検知された位置の変化量が、外力算出部２１７において、第二リンク１８４、第三リンク１８５が操作されて発生した位置の変化ではないと判定された場合（Ｓ１１０２／Ｎｏ）、駆動制御部２１１は、本処理を終了させる。外力算出部２１７は、例えば、マニピュレータ装置１がベース１９９ごと動かされて生じた位置の変化量については、第二リンク１８４、第三リンク１８５が操作されて発生した位置の変化ではないと判定する。 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. .
検知された位置の変化量が、外力算出部２１７において、第二リンク１８４、第三リンク１８５が操作されて発生した位置の変化であると判定された場合（Ｓ１１０２／Ｙｅｓ）、駆動制御部２１１は、検知された位置の変化量に基づいて第二リンク１８４、第三リンク１８５が移動するように第一モータ１８８、第二モータ１９１に出力されるモータ出力値を算出する（Ｓ１１０３）。 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).
この時、駆動制御部２１１は、検知された位置の変化量に基づいて駆動部１０、１１を駆動させるために、電流値と駆動部１０、１１のトルク定数を乗算した値（モータ出力値）を回転駆動部２１２に出力する。回転駆動部２１２は、モータ出力値に基づいて、駆動部１０、１１を駆動させる（Ｓ１１０４）。 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).
駆動部１０、１１がモータ出力値に基づいて回転駆動することによって、第二リンク１８４、第三リンク１８５は移動される。 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.
以上説明したように、本実施形態においては、第二リンク１８４、第三リンク１８５の移動状況に応じて、ユーザが移動させようとする方向にアシスト制御を行うため、マニピュレータ装置１の操作性を向上させることが出来る。従って、マニピュレータ装置１の質量や、第一モータ１８８、第二モータ１９１のトルクが大きく、ユーザが意図する操作が行いにくい場合でも、ユーザが操作しようとする方向に第二リンク１８４、第三リンク１８５が移動するようにアシスト制御を行う。そのため、ユーザは第二リンク１８４、第三リンク１８５を操作する際に、小さな力を加えるだけで、マニピュレータ装置１を動かすことが出来る。 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.
また、ユーザの操作によっては、第二リンク１８４、第三リンク１８５が突然移動する等して、ユーザが意図しない方向に、マニピュレータ装置１が移動することも考えられる。これに対して、本実施形態に係る制御装置２においては、図１２に示すように、第二リンク１８４、第三リンク１８５を移動させるために必要な移動力目標値を設定し、外力が不感帯を持つように設定する。さらに、予めマニピュレータ装置１が移動可能な位置の範囲や移動速度の設定値である位置速度指令値を設定する。 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.
このような移動力目標値、位置速度指令値を設定してマニピュレータ装置１の動作を制御する処理について以下、図１６を参照して説明する。位置速度指令生成部２２２は、制御装置２に入力されたマニピュレータ装置１が移動可能な範囲や移動速度の設定値の情報に基づいて、位置速度指令値を設定する（Ｓ１３０１）。さらに、移動力目標生成部２２１は、制御装置２に入力された第二リンク１８４、第三リンク１８５を移動させるために必要な移動力目標値の情報に基づいて、移動力目標値を設定する（Ｓ１３０２）。尚、位置速度指令値または移動力目標値は何れか一つを設定する構成であってもよい。 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.
位置変化量検知部２１９において位置の変化量が検知される（Ｓ１３０３）と、外力算出部２１７は、第二リンク１８４、第三リンク１８５が移動されることで発生した位置の変化量であるか否か判定する（Ｓ１３０４）。 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).
検知された位置の変化量が、外力算出部２１７において、第二リンク１８４、第三リンク１８５が操作されて発生した位置の変化ではないと判定された場合（Ｓ１３０４／Ｎｏ）、駆動制御部２１１は、本処理を終了させる。外力算出部２１７は、図１１と同様に、例えば、マニピュレータ装置１がベース１９９ごと動かされて生じた位置の変化量については、第二リンク１８４、第三リンク１８５が操作されて発生した位置の変化ではないと判定する。 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.
検知された位置の変化量が、外力算出部２１７において、第二リンク１８４、第三リンク１８５が操作されて発生した位置の変化量であると判定された場合（Ｓ１３０４／Ｙｅｓ）、位置情報取得部２１６は、駆動部１０、１１の位置に基づいて、第二リンク１８４、第三リンク１８５の位置情報を出力する。 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.
移動力目標生成部２２１は、位置情報及び外力算出部２１７において検知された位置の変化量に基づいて、Ｓ１３０２で設定した移動力目標値と外力の推定値とを比較する。そして、この比較結果に基づいて、移動力目標情報を生成し、駆動制御部２１１に送信する（Ｓ１３０５、移動力目標値誤差検知）。 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).
駆動制御部２１１は、移動力目標情報に基づいて第二リンク１８４、第三リンク１８５が移動力で移動するように、駆動部１０、１１を制御する。また、位置速度指令生成部２２２は、Ｓ１６０１にて設定した位置速度指令値とマニピュレータ装置１の移動情報及び移動速度とを比較する。そして、この比較結果に基づいて、位置速度指令情報を生成し、駆動制御部２１１に送信する（Ｓ１３０６）。 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).
駆動制御部２１１は、推定された外力と同じ方向にアシスト制御が行われるように第一モータ１８８、第二モータ１９１に出力されるモータ出力値を回転駆動部２１２に出力する（Ｓ１３０７）。この時、検知された位置の変化量に基づいて駆動部１０、１１を駆動させるために、電流値と駆動部１０、１１のトルク定数を乗算した値と、Ｓ１３０５で生成された新たな移動力目標情報とＳ１３０６で生成された位置・速度を指定する情報とを参照して算出される。回転駆動部２１２は、モータ出力値に基づいて駆動部１０、１１を駆動させる（Ｓ１３０８）。 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).
駆動部１０、１１がモータ出力値に基づいて回転駆動することによって、第二リンク１８４、第三リンク１８５は、Ｓ１３０５で生成された新たな移動力目標情報とＳ１３０６で生成された位置・速度を指定する情報に基づいて移動される。 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.
以上説明したように、制御装置２に入力される移動力目標値及び位置速度設定値との誤差をフィードバックし、第二リンク１８４、第三リンク１８５を移動させるために、駆動部１０、１１の回転駆動のアシスト制御を実行する。このようにすることで、マニピュレータ装置１が突然動作することや、意図しない位置に移動することを抑制できる。また、移動力目標値を設定する範囲によっては、マニピュレータ装置１を所定の移動力を加えることで移動させることが可能である。 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.
以上説明したアシスト制御の処理においては、外力と同じ方向のベクトルにマニピュレータ装置１を直接教示する場合を例として説明を行った。これらの処理においては、推定された外力と逆方向（逆ベクトル）にマニピュレータ装置１が移動するように機能を切り替える制御を行うことも可能である。尚、マニピュレータ装置１がアシスト制御される方向を切り替える制御は、スイッチ２８をＯＮ／ＯＦＦ操作して切り替える構成であってもよい。また、推定された外力が所定以上である場合や回転速度を検知した場合に、アシスト制御を実行しない構成であってもよい。 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.
尚、マニピュレータ装置１を軽量化または小型化するために、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４の代わりに、第一ワイヤ１８６の伸縮状況に基づいて第二リンク１８４、第三リンク１８５の移動量を検知してもよい。図１４は、本実施形態に係る第一ワイヤ１８６に取り付けられるひずみゲージ１８６Ｂを示す図である。 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.
図１４に示すように、ひずみゲージ１８６Ｂは、第一ワイヤ１８６の直線部分と接続されたプレート１８６Ａの上に設置されており、第一ワイヤ１８６の伸縮を電気的に検知することが出来る。従って、ひずみゲージ１８６Ｂは、ワイヤの伸縮量を検知する伸縮検知部として機能する。マニピュレータ装置１のユーザが、第二リンク１８４、第三リンク１８５を移動させた場合に、制御装置２とひずみゲージ１８６Ｂとに接続されている電気回路および伝達経路によって第一ワイヤ１８６の伸縮量が、外力算出部２１７に伝達される。 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.
外力算出部２１７は、ひずみゲージ１８６Ｂから伝達された第一ワイヤ１８６の伸縮量に基づいて、マニピュレータ装置１に加えられた外力の値を推定する。尚、第二ワイヤ１８７についても同様の構成にすることによって、第二ワイヤ１８７の伸縮量を外力算出部２１７に伝達することが出来る。 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.
また、マニピュレータ装置１を軽量化または小型化するために、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４の代わりに、第一ワイヤ１８６の張力調整部１３５によって第二リンク１８４、第三リンク１８５の移動量を検知することも出来る。図１５は、本実施形態に係る第一ワイヤ１８６に取り付けられる張力調整部１３５の構造を示す図である。 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.
図１５に示すように、張力調整部１３５は、張力検知部１３１、アイドラプーリ１３２、１３３、１３４を含む。張力検知部１３１は、図１４で説明したひずみゲージ１８６Ｂを含んで構成され、第二リンク１８４、第三リンク１８５が移動させられて第一ワイヤ１８６に生じる張力の変化を検知する。また、アイドラプーリ１３２、１３３、１３４は、第一ワイヤ１８６を這いまわすように配置される。 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.
外力算出部２１７は、張力検知部１３１から伝達された第一ワイヤ１８６の伸縮量に基づいて、マニピュレータ装置１に加えられた外力の値を推定する。尚、第二ワイヤ１８７についても同様の構成にすることによって、第二ワイヤ１８７の伸縮量を外力算出部２１７に伝達することが出来る。 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.
さらに、第二リンク１８４、第三リンク１８５の移動方向を検知するために、図１６及び図１７に示すような移動方向検知部材５００を用いてもよい。図１６は、移動方向検知部材５００に用いられる回転ダンパ５０１の構成を示す図、図１７は、本実施形態に係る移動方向検知部材５００である。 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.
図１６に示すように、回転ダンパ５０１は、入力軸５０１Ａ、出力軸５０１Ｂ、シャッタ部５０１Ｃを含む。シャッタ部５０１Ｃは、出力軸５０１Ｂに付加され構成されている。また、図１７に示すように、移動方向検知部材５００は、第二リンク１８４、第三リンク１８５にそれぞれ装着されており、回転ダンパ５０１、ブラケット５０２、フォトインタラプタ５０３、スプリングプランジャ５０４を含む。 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.
回転ダンパ５０１は、第一従動プーリ１９５、第二従動プーリ１９８にそれぞれ装着されている。移動方向検知部材５００は、ブラケット５０２によって第二リンク１８４、第三リンク１８５に固定されている。また、フォトインタラプタ５０３およびスプリングプランジャ５０４はブラケット５０２上に配置されている。 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.
フォトインタラプタ５０３は、対向する発光部と受光部とから構成され、発光部から照射される光を受光部で検出する際の検出状況に応じて物体の有無あるいは位置を判定するセンサである。スプリングプランジャ５０４は、対向する弾性体によってシャッタ部５０１Ｃを保持する保持部材である。 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.
図１６に示す例においては、２つのフォトインタラプタ５０３がどちらもＯＦＦである中立位置を表している。この状態からマニピュレータ装置１が操作された場合、入力軸５０１Ａの回転が出力軸５０１Ｂに伝わり、シャッタ部５０１Ｃが回転する際の正または負の回転方向に応じて２つのフォトインタラプタ５０３のうち、いずれかをＯＮにする。 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.
出力軸５０１Ｂの位相は、シャッタ部５０１Ｃが両側をスプリングプランジャ５０４によって押圧されているため、フォトインタラプタ５０３の検知位置からシャッタ部５０１Ｃが抜去されない範囲で動作する。入力軸５０１Ａの回転が停止すると、弾性体の作用で出力軸５０１Ｂは中立位置に復帰し、次回入力軸５０１Ａが回転するまで停止する。 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.
このように、従動プーリに伝達される駆動力の伝達方向に応じて制御されるフォトインタラプタ５０３のＯＮ信号に基づいてアシスト制御を開始することによって、簡単な回路構成およびアルゴリズムを実装したマニピュレータ装置１の制御装置２を実現することが出来る。以上説明したように、移動方向検知部材５００は、従動プーリに伝達される駆動力の伝達方向に応じて第二リンク１８４及び第三リンク１８５の移動方向をそれぞれ検知する移動方向検知部として機能する。 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. .
尚、入力軸５０１Ａと接続することが可能であれば、例えば、第一駆動プーリ１９４、第二駆動プーリ１９６や第一モータ減速装置１９０、第二モータ減速装置１９３もしくはモータ軸等に移動方向検知部材５００を装着する構成であってもよい。 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.
また、移動方向を検知するために、マグネット式もしくはコイルスプリング式もしくはフリクション式のトルクリミッタを移動方向検知部材５００として使用してもよい。さらに、ストローク型ダンパを回転ダンパ５０１の代わりに使用することも出来る。ストローク型ダンパを用いる場合には、マニピュレータ装置１における回転部材の他に、直線運動を行う部材にも適用することができるため、例えば、第一ワイヤ１８６、第二ワイヤ１８７や張力調整部１３５に対して適用することが可能である。 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.
尚、Ｓ１１０１及びＳ１３０３の処理においては、マニピュレータ装置１が停止状態から移動されたタイミングにおいて位置の変化量を検知してもよい。このようにすると、マニピュレータ装置１が動き出す際にアシスト制御を行うことが出来るため、ユーザが第二リンク１８４、第三リンク１８５を操作するために必要な力を小さくすることが出来る。 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.
１ マニピュレータ装置
１０、１１ 駆動部
２１ ＣＰＵ
２２ ＲＡＭ
２３ ＲＯＭ
２４ エンジン
２５ ＨＤＤ
２６ Ｉ／Ｆ
２７ センサ
２８ スイッチ
２９ バス
１３５ 張力調整部
１７１ 第一関節トルクリミッタ
１７２ 第二関節トルクリミッタ
１７３ 第一関節軸エンコーダ
１７４ 第二関節軸エンコーダ
１８１ 第一関節
１８２ 第二関節
１８３ 第一リンク
１８４ 第二リンク
１８５ 第三リンク
１８６ 第一ワイヤ
１８７ 第二ワイヤ
１８８ 第一モータ
１８９ 第一モータエンコーダ
１９０ 第一モータ減速装置
１９１ 第二モータ
１９２ 第二モータエンコーダ
１９３ 第二モータ減速装置
１９４ 第一駆動プーリ
１９５ 第一従動プーリ
１９６ 第二駆動プーリ
１９７ 第二中継プーリ
１９８ 第二従動プーリ
１９９ ベース
１８６Ｂ ひずみゲージ
２０１ 回転駆動制御部
２１１ 駆動制御部
２１２ 回転駆動部
２１４ 回転検知部
２１５ ＦＢ取得部
２１６ 位置情報取得部
２１７ 外力算出部
２１８ 外力検知部
２１９ 位置変化量検知部
２２１ 移動力目標生成部
２２２ 位置速度指令生成部
５００ 移動方向検知部
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
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.
前記移動状況が変化したタイミングに前記移動部が移動させられた方向に前記移動部が移動するように前記駆動部を制御することを特徴とする請求項１に記載のマニピュレータ装置の制御装置。 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.
前記移動部が停止状態から移動させられたタイミングに前記移動部が移動させられた方向に前記移動部が移動するように前記駆動部を制御することを特徴とする請求項１または請求項２に記載のマニピュレータ装置の制御装置。 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.
前記外力算出部は、
前記駆動伝達部の伸縮状況に基づいて前記交差箇所における前記移動状況の変化を検知し、検知した前記移動状況の変化に基づいて前記推定値を算出することを特徴とする請求項１ないし請求項３いずれか一項に記載のマニピュレータ装置の制御装置。 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.
前記外力算出部は、
前記交差箇所における前記移動状況の光学的な変化に基づいて前記推定値を算出することを特徴とする請求項１ないし請求項３いずれか一項に記載のマニピュレータ装置の制御装置。 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.
前記外力算出部は、
検知された前記伸縮量に基づいて前記交差箇所における前記移動状況の変化を検知し、検知した前記移動状況の変化に基づいて前記推定値を算出することを特徴とする請求項１ないし請求項３いずれか一項に記載のマニピュレータ装置の制御装置。 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.
前記移動方向検知部は、
前記従動部に前記駆動部の動力を伝達する際の伝達方向に基づいて前記移動部の移動方向を検知することを特徴とする請求項１ないし請求項３いずれか一項に記載のマニピュレータ装置の制御装置。 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.
前記移動方向検知部は、
前記伸縮量に基づいて前記移動部の移動方向を検知することを特徴とする請求項６に記載のマニピュレータ装置の制御装置。 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
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 (3)
| 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 |
| US11806874B2 (en) | 2019-10-18 | 2023-11-07 | Fanuc Corporation | Robot system |
| JP7529599B2 (en) | 2021-03-24 | 2024-08-06 | 本田技研工業株式会社 | robot |
-
2016
- 2016-05-18 JP JP2016099918A patent/JP2017205835A/en active Pending
Cited By (3)
| 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 |
| US11806874B2 (en) | 2019-10-18 | 2023-11-07 | Fanuc Corporation | Robot system |
| JP7529599B2 (en) | 2021-03-24 | 2024-08-06 | 本田技研工業株式会社 | robot |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP5895628B2 (en) | ROBOT CONTROL METHOD, ROBOT CONTROL DEVICE, AND ROBOT CONTROL SYSTEM | |
| JP5383911B2 (en) | Robot controller | |
| JP2016013613A (en) | Robot control method, robot device, program, recording medium, and method for manufacturing assembled component | |
| US20160263749A1 (en) | Joint driving apparatus and robot apparatus | |
| US20150217445A1 (en) | Method For Programming An Industrial Robot and Industrial Robot | |
| US11839979B2 (en) | Dual mode free-drive of robot arm | |
| WO2015101088A1 (en) | Intelligent control method, device and system for multi-joint mechanical arm | |
| SE0301531D0 (en) | A Control method for a robot | |
| JP2011101938A (en) | Robot and control device for the same | |
| DK180508B1 (en) | Maintaining free-drive mode of robot arm for period of time | |
| JP2017205835A (en) | Control device of manipulator device, control method of the manipulator device, and control program of the manipulator device | |
| TWI651175B (en) | Control device of robot arm and teaching system and method using the same | |
| WO2015090324A1 (en) | Device for dynamic switching of robot control points | |
| KR102422355B1 (en) | An remote control system based on augmented reality for underater robot sterilization robot based on 3d environmental recognition | |
| JP4842561B2 (en) | Force controller device | |
| JP2012051043A (en) | Robot system or robot control device | |
| WO2016162066A1 (en) | An industrial robot and a method for lead-through programming of an industrial robot | |
| JP2017164833A (en) | Control device of manipulator device, control method of manipulator device, and control program of manipulator device | |
| JP2008528307A (en) | 5-bar mechanism with dynamic balancing means and method for dynamically balancing a 5-bar mechanism | |
| JP2021088042A (en) | Robot control device, gripping system and robot hand control method | |
| KR101474778B1 (en) | Control device using motion recognition in artculated robot and method thereof | |
| JP2017035756A (en) | Robot control device suppressing vibration of tool tip end of robot having travel shaft | |
| JP5633268B2 (en) | Robot control device | |
| JP6831208B2 (en) | Robot control device | |
| US20230035296A1 (en) | Method of suppressing vibrations of a robot arm with external objects |