JP2016039737A - Motor controller and motor control method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To achieve a flexible and highly accurate joint control.SOLUTION: A motor controller (1) includes: a dynamic friction compensator (6) for generating a dynamic friction compensatory signal and provide it to a motor (2) on the basis of a rotational speed signal detected with a rotational speed sensor (4) and a predetermined dynamic friction model of a reduction gear (3); and a static friction compensator (7) for generating a static friction compensatory signal on the basis of a torque signal detected with a torque sensor (5) and providing it to the motor (2).SELECTED DRAWING: Figure 1

Description

本発明は、減速機を有するモータを制御するモータ制御装置及びモータ制御方法に関する。   The present invention relates to a motor control device and a motor control method for controlling a motor having a reduction gear.

産業用途に代表される従来のロボットの多くは、精密な位置決めを必要とする作業の遂行を目的に設計されてきた。そのため、これらのロボットの関節は高ゲインなフィードバック補償によって硬く制御され、事故を避けるために、人と隔離された空間に置く必要があった。   Many conventional robots represented by industrial applications have been designed for the purpose of performing work that requires precise positioning. For this reason, the joints of these robots are rigidly controlled by high gain feedback compensation, and it is necessary to place them in a space separated from humans in order to avoid accidents.

しかしながら、近年、人と環境を共有し相互に関わり合い、人の作業を支援するロボットが求められている。このようなロボットの関節は、作業の遂行に十分な位置決め精度を確保するための剛性、及び、接触時における柔軟性という相反する性質を兼ね備えている必要がある。   However, in recent years, there has been a demand for robots that share the environment with people, interact with each other, and support human work. Such a robot joint needs to have the contradictory properties of rigidity for ensuring sufficient positioning accuracy for performing work and flexibility at the time of contact.

上記柔軟性を確保する方法としては、ばね・ダンパ等の機構的柔軟要素の利用にすぐに思い至る（非特許文献１）。しかし、このような機構的柔軟要素は制御精度の著しい低下を招く。剛性と柔軟性とを両立するためには、アクチュエータとその力の伝達機構がバックドライバビリティを有していなければならない。   As a method of ensuring the above flexibility, the use of mechanical flexible elements such as springs and dampers immediately comes to mind (Non-Patent Document 1). However, such a mechanical flexible element causes a significant decrease in control accuracy. In order to achieve both rigidity and flexibility, the actuator and its force transmission mechanism must have back drivability.

ここで、バックドライバビリティとは、出力節に力を加えたときに、当該出力節が可動し、且つ、当該出力節に加えられた力が入力節側に伝わり、駆動機を逆駆動できることを意味するものとする。   Here, back drivability means that when a force is applied to an output node, the output node is movable, and the force applied to the output node is transmitted to the input node side, so that the drive machine can be driven in reverse. Shall mean.

従来のロボットでは回転型電磁モータに高減速比減速機を組み合わせた小型な構成で、高トルク・低回転数の特性を得ている。しかしながら、高い減速比は減速機内部の摩擦をも増幅するため、バックドライバビリティを損なう直接的原因となる。摩擦の発生を避けるために、ダイレクトドライブモータを利用した研究もあるが（非特許文献２）、関節の過度な大型化を招いた。電磁駆動によるリニアドライブアクチュエータを用いた研究もあるが（非特許文献３）、十分な推力が得られない。油圧駆動によるリニアドライブアクチュエータ（非特許文献４）も開発されているが、高価であり必ずしも広く利用できるものではない。   Conventional robots have high torque and low rotational speed characteristics with a compact configuration combining a rotary electromagnetic motor with a high reduction ratio reduction gear. However, since a high reduction ratio also amplifies the friction inside the reduction gear, it is a direct cause of impairing back drivability. In order to avoid the occurrence of friction, there is a research using a direct drive motor (Non-Patent Document 2), but this has caused an excessive increase in size of the joint. There is also research using a linear drive actuator by electromagnetic drive (Non-patent Document 3), but sufficient thrust cannot be obtained. A hydraulic drive linear drive actuator (Non-Patent Document 4) has also been developed, but is expensive and not necessarily widely available.

このように現時点においては、ハードウェアのみで、この剛性と柔軟性とを兼ね備えるという要求特性を実現することは難しい。一方、従来の回転型電磁モータと高減速比減速機との組み合わせにおいて、減速機内部で生じる摩擦力を補償して剛性と柔軟性とを両立するという、問題をソフトウェア的に解決する方法が、近年いくつか報告されている（非特許文献５、非特許文献６、非特許文献７）。   Thus, at the present time, it is difficult to achieve the required characteristics of having both rigidity and flexibility only with hardware. On the other hand, in a combination of a conventional rotary electromagnetic motor and a high reduction ratio reducer, a method of solving the problem of software that compensates for the friction force generated inside the reducer and achieves both rigidity and flexibility, Several reports have been made in recent years (Non-patent document 5, Non-patent document 6, Non-patent document 7).

また、減速機内部で生じる動摩擦力をフィードフォワード制御により補償する構成が開示されている（非特許文献８、特許文献１、特許文献２）。   Moreover, the structure which compensates the dynamic friction force which arises inside a reduction gear by feedforward control is disclosed (nonpatent literature 8, patent documents 1, patent documents 2).

特許第4985659号明細書（2012年5月11日登録）Patent No.4985659 specification (registered on May 11, 2012) 特許第3752758号明細書（2005年12月22日登録）Patent No. 3752758 (Registered on December 22, 2005)

G. Pratt, M. Williamson, P. Dillworth, J. Pratt, K. Ullandand A. Wright，“Stiffness Isn’t Everything,” Preprintsof the Fourth International Symposium on ExperimentalRobotics, 1995.G. Pratt, M. Williamson, P. Dillworth, J. Pratt, K. Ullandand A. Wright, “Stiffness Is n’t Everything,” Preprintsof the Fourth International Symposium on Experimental Robotics, 1995. H. Asada, T. Kanade, I. Takeyama, “Control of a Direct-Drive Arm,” Journal of Dynamic Systems, Measurement, and Control, Vol.105, 1983, pp.136-242.H. Asada, T. Kanade, I. Takeyama, “Control of a Direct-Drive Arm,” Journal of Dynamic Systems, Measurement, and Control, Vol.105, 1983, pp.136-242. Yoshihiro NAKATA, Tianyi YU, Hiroshi ISHIGURO, Katsuhiro HIRATA “Small Size Linear Vernier Motor for Artificial Muscle Applications” The Japan Society of Applied Electromagnetics and Mechanics, Vol. 20, No.1, 2012, pp.113-118.Yoshihiro NAKATA, Tianyi YU, Hiroshi ISHIGURO, Katsuhiro HIRATA “Small Size Linear Vernier Motor for Artificial Muscle Applications” The Japan Society of Applied Electromagnetics and Mechanics, Vol. 20, No.1, 2012, pp.113-118. Hiroshi Kaminaga, Tomoya Amari, Yukihiro Katayama, Junya Ono, Yuto Shimoyama, and Yoshihiro Nakayama, “Backdrivability Analysis of Electro-Hydrostatic Actuator and Series Dissipative Actuation Model” 2010 IEEE International Conference on Robotics and Automation, Anchorage, Alaska, USA, May 3-8, 2010, pp.4204-4211.Hiroshi Kaminaga, Tomoya Amari, Yukihiro Katayama, Junya Ono, Yuto Shimoyama, and Yoshihiro Nakayama, “Backdrivability Analysis of Electro-Hydrostatic Actuator and Series Dissipative Actuation Model” 2010 IEEE International Conference on Robotics and Automation, Anchorage, Alaska, USA, May 3 -8, 2010, pp.4204-4211. G. Hirzinger, N. Sporner, A.Albu-Schaffer, M. Hahnle, R. Krenn, A. Pascucci, M. Schedl “DLR’s torque-controlled light weight robot III: Are we reaching the technological limits now？”Proceedings of the 2002 IEEE International Conference on Robotics and Automation Washington, DC, USA,・May 2002, pp.1710-1716.G. Hirzinger, N. Sporner, A. Albu-Schaffer, M. Hahnle, R. Krenn, A. Pascucci, M. Schedl “DLR's torque-controlled light weight robot III: Are we reaching the technological limits now?” Proceedings of the 2002 IEEE International Conference on Robotics and Automation Washington, DC, USA, May 2002, pp. 1710-1716. T. Kawakami, K. Ayusawa, H. Kaminaga and YoshihikoNakamura, “High-Fidelity Joint Drive System by Torque Feedback Control Using High Precision Linear Encoder,”Proceedings of 2010 IEEE International Conference on Robotics and Automation, Anchorage, Alaska, USA, May 3-8, 2010, pp.3904-3909.T. Kawakami, K. Ayusawa, H. Kaminaga and Yoshihiko Nakamura, “High-Fidelity Joint Drive System by Torque Feedback Control Using High Precision Linear Encoder,” Proceedings of 2010 IEEE International Conference on Robotics and Automation, Anchorage, Alaska, USA, May 3-8, 2010, pp.3904-3909. Kenichiro Nagasaki, Yasunori Kawanami, Satoru Shimizu, Takashi Kito, Toshimitsu Tsuboi, Atsushi Miyamoto, Tetsuharu Fukushima and Hideki Shimomura(Sony Corporation) “Whole-body Cooperative Force Control for a Two-Wheeled Mobile Robot Using Generalized Inverse Dynamics and Idealized Joint Units” 2010 IEEE International Conference on Robotics and Automation, Anchorage, Alaska, USA, May 3-8, 2010, pp.3377-3383.Kenichiro Nagasaki, Yasunori Kawanami, Satoru Shimizu, Takashi Kito, Toshimitsu Tsuboi, Atsushi Miyamoto, Tetsuharu Fukushima and Hideki Shimomura (Sony Corporation) “Whole-body Cooperative Force Control for a Two-Wheeled Mobile Robot Using Generalized Inverse Dynamics and Idealized Joint Units” 2010 IEEE International Conference on Robotics and Automation, Anchorage, Alaska, USA, May 3-8, 2010, pp.3377-3383. Oussama Khatib, Peter Thaulad, Taizo Yoshizawa, and Jachcung Park (Stanford University, Artificial Intelligence Laboratory) (Honda Research Institute USA, Inc.) “Torque-Position Transformer for Task Control of Position Controlled Robots” 2008 IEEE International Conference on Robotics and Automation Pasadena, CA, USA, May 19-23, 2008, pp.1729-1734.Oussama Khatib, Peter Thaulad, Taizo Yoshizawa, and Jachcung Park (Stanford University, Artificial Intelligence Laboratory) (Honda Research Institute USA, Inc.) “Torque-Position Transformer for Task Control of Position Controlled Robots” 2008 IEEE International Conference on Robotics and Automation Pasadena, CA, USA, May 19-23, 2008, pp.1729-1734.

しかしながら、上記非特許文献３〜５に開示された構成は、いずれもトルクを計測し加速度レベルでの制御を行うもので、高精度な（従って高価な）トルクセンサの使用を前提としており、やはり広く利用できるものではない。   However, the configurations disclosed in Non-Patent Documents 3 to 5 above all measure torque and control at the acceleration level, and are based on the use of a highly accurate (and therefore expensive) torque sensor. It is not widely available.

また、非特許文献６、特許文献１〜２に教示された構成では、減速機内部で生じる静止摩擦力を補償することができない。   Further, the configurations taught in Non-Patent Document 6 and Patent Documents 1 and 2 cannot compensate for the static friction force generated inside the reduction gear.

本発明は、回転型電磁モータと高減速比減速機との組み合わせにおいて、柔軟かつ高精度な関節制御を、安価なハードウェア構成とソフトウェアとにより実現することを目的とする。   An object of the present invention is to realize flexible and highly accurate joint control with an inexpensive hardware configuration and software in a combination of a rotary electromagnetic motor and a high reduction ratio reduction gear.

上記の課題を解決するために、本発明に係るモータ制御装置は、減速機を有するモータを制御するモータ制御装置であって、前記減速機の出力軸の回転速度を検出する回転速度センサと、前記減速機の出力軸のトルクを検出するトルクセンサと、前記回転速度センサにより検出された前記回転速度を表す回転速度信号と、予め定められた前記減速機の動摩擦モデルとに基づいて、前記減速機の動摩擦トルクを補償する動摩擦補償信号を生成して前記モータに供給する動摩擦補償器と、前記トルクセンサにより検出された前記トルクを表すトルク信号と、前記減速機の出力軸の回転速度に対応する角加速度を表す角加速度信号に基づいて生成されるモータ慣性トルク信号とに基づいて、前記減速機の静止摩擦トルクを補償する静止摩擦補償信号を生成して前記モータに供給する静止摩擦補償器とを備えたことを特徴とする。   In order to solve the above problems, a motor control device according to the present invention is a motor control device that controls a motor having a speed reducer, and a rotational speed sensor that detects a rotational speed of an output shaft of the speed reducer; Based on a torque sensor that detects torque of the output shaft of the speed reducer, a rotational speed signal that represents the rotational speed detected by the rotational speed sensor, and a predetermined dynamic friction model of the speed reducer Corresponding to a dynamic friction compensator that generates a dynamic friction compensation signal that compensates the dynamic friction torque of the machine and supplies it to the motor, a torque signal that represents the torque detected by the torque sensor, and a rotational speed of the output shaft of the speed reducer A static friction compensation signal for compensating for the static friction torque of the reduction gear based on a motor inertia torque signal generated based on an angular acceleration signal representing the angular acceleration Generates and characterized in that a static friction compensator supplied to the motor.

この特徴によれば、減速機の動摩擦トルクは動摩擦補償器により補償する。このため、減速機の静止摩擦トルクを補償する静止摩擦補償器のためのトルクセンサは、バンド幅の小さい安価なトルクセンサを採用することが可能になる。この結果、安価なハードウェア構成とソフトウェアとにより、柔軟かつ高精度な関節制御を実現することができる。   According to this feature, the dynamic friction torque of the speed reducer is compensated by the dynamic friction compensator. For this reason, an inexpensive torque sensor with a small bandwidth can be adopted as the torque sensor for the static friction compensator that compensates for the static friction torque of the reduction gear. As a result, flexible and highly accurate joint control can be realized with an inexpensive hardware configuration and software.

本発明に係るモータ制御装置は、前記動摩擦モデルは、前記モータへの印加電圧を前記減速機の出力軸の回転速度についての関数として与えられることが好ましい。   In the motor control device according to the present invention, it is preferable that the dynamic friction model is provided with a voltage applied to the motor as a function of a rotation speed of an output shaft of the speed reducer.

上記構成によれば、回転速度センサに基づく減速機の出力軸の回転速度と予め定められた減速機の動摩擦モデルとに基づいて、減速機の動摩擦トルクを補償する動摩擦補償信号を、モータへの入力信号に予め印加することができる。   According to the above configuration, the dynamic friction compensation signal for compensating the dynamic friction torque of the speed reducer is supplied to the motor based on the rotational speed of the output shaft of the speed reducer based on the rotational speed sensor and the predetermined dynamic friction model of the speed reducer. It can be applied in advance to the input signal.

本発明に係るモータ制御装置は、前記静止摩擦補償信号は、前記モータ慣性トルク信号と、前記トルク信号に基づいて前記トルクを推定するトルク推定信号と、前記モータへの印加電圧を表す印加電圧信号とに基づいて生成されることが好ましい。   In the motor control device according to the present invention, the static friction compensation signal includes the motor inertia torque signal, a torque estimation signal for estimating the torque based on the torque signal, and an applied voltage signal representing an applied voltage to the motor. It is preferable to be generated based on the above.

上記構成によれば、減速機の静止摩擦トルクを補償する静止摩擦補償信号を、モータへの入力信号にフィードバックすることができる。   According to the above configuration, the static friction compensation signal for compensating the static friction torque of the reduction gear can be fed back to the input signal to the motor.

本発明に係るモータ制御装置では、前記減速機は、歯車減速機であることが好ましい。   In the motor control device according to the present invention, it is preferable that the speed reducer is a gear speed reducer.

上記構成によれば、簡単な構成により、モータの回転を高減速比で減速することができる。   According to the above configuration, the rotation of the motor can be decelerated at a high reduction ratio with a simple configuration.

本発明に係るモータ制御装置では、前記歯車減速機は、波動歯車減速機であることが好ましい。   In the motor control device according to the present invention, the gear reducer is preferably a wave gear reducer.

上記構成によれば、簡単な構成により、モータの回転を高減速比で減速することができる。   According to the above configuration, the rotation of the motor can be decelerated at a high reduction ratio with a simple configuration.

本発明に係るモータ制御方法は、減速機を有するモータを制御するモータ制御方法であって、前記減速機の出力軸の回転速度を検出する回転速度検出工程と、前記減速機の出力軸のトルクを検出するトルク検出工程と、前記回転速度検出工程により検出された前記回転速度を表す回転速度信号と、予め定められた前記減速機の動摩擦モデルとに基づいて、前記減速機の動摩擦トルクを補償する動摩擦補償信号を生成して前記モータに供給する動摩擦補償工程と、前記トルク検出工程により検出された前記トルクを表すトルク信号と、前記減速機の出力軸の回転速度に対応する角加速度を表す角加速度信号に基づいて生成されるモータ慣性トルク信号とに基づいて、前記減速機の静止摩擦トルクを補償する静止摩擦補償信号を生成して前記モータに供給する静止摩擦補償工程とを包含することを特徴とする。   A motor control method according to the present invention is a motor control method for controlling a motor having a speed reducer, the rotational speed detecting step for detecting the rotational speed of the output shaft of the speed reducer, and the torque of the output shaft of the speed reducer. Compensation of the dynamic friction torque of the speed reducer based on a torque detection step for detecting the rotational speed, a rotational speed signal representing the rotational speed detected by the rotational speed detection step, and a predetermined dynamic friction model of the speed reducer A dynamic friction compensation step that generates and supplies the dynamic friction compensation signal to the motor, a torque signal that represents the torque detected by the torque detection step, and an angular acceleration that corresponds to the rotational speed of the output shaft of the speed reducer Based on the motor inertia torque signal generated based on the angular acceleration signal, a static friction compensation signal for compensating the static friction torque of the reduction gear is generated to generate the mode. Characterized in that it comprises a static friction compensation step of supplying a.

本発明は、回転型電磁モータと高減速比減速機との組み合わせにおいて、柔軟かつ高精度な関節制御を、安価なハードウェア構成とソフトウェアとにより実現することができるという効果を奏する。   The present invention produces an effect that flexible and highly accurate joint control can be realized by an inexpensive hardware configuration and software in a combination of a rotary electromagnetic motor and a high reduction ratio reduction gear.

実施形態に係るモータ制御装置の構成を示すブロック図である。It is a block diagram which shows the structure of the motor control apparatus which concerns on embodiment. 上記モータ制御装置に設けられた動摩擦補償器による摩擦補償を説明するためのブロック線図である。It is a block diagram for demonstrating the friction compensation by the dynamic friction compensator provided in the said motor control apparatus. 上記モータ制御装置に設けられた静止摩擦補償器による摩擦補償及び上記動摩擦補償器による摩擦補償を説明するためのブロック線図である。It is a block diagram for demonstrating the friction compensation by the static friction compensator provided in the said motor control apparatus, and the friction compensation by the said dynamic friction compensator. （ａ）（ｂ）は上記モータ制御装置の実験装置に設けられたモータ及び減速機の構成を示す斜視図である。(A) (b) is a perspective view which shows the structure of the motor and speed reducer which were provided in the experimental apparatus of the said motor control apparatus. 上記モータ制御装置の実験装置に設けられたトルクセンサの構成を示す斜視図である。It is a perspective view which shows the structure of the torque sensor provided in the experimental apparatus of the said motor control apparatus. （ａ）（ｂ）は上記トルクセンサの動作を説明するための図である。(A) (b) is a figure for demonstrating operation | movement of the said torque sensor. 上記モータ制御装置に設けられた減速機の出力軸の角速度と上記出力軸のトルクを表すＰＷＭデューティ比との関係を示すグラフである。It is a graph which shows the relationship between the angular velocity of the output shaft of the reduction gear provided in the said motor control apparatus, and the PWM duty ratio showing the torque of the said output shaft. 上記トルクセンサの出力電圧と上記出力軸のトルクとの間の関係を示すグラフである。It is a graph which shows the relationship between the output voltage of the said torque sensor, and the torque of the said output shaft. 上記モータ制御装置の実験装置に設けられたフィードバック（ＰＩＤ）制御器とフィードフォワード制御器からなる制御システムのブロック線図である。It is a block diagram of the control system which consists of the feedback (PID) controller and feedforward controller which were provided in the experimental apparatus of the said motor control apparatus. （ａ）〜（ｄ）は上記制御システムにより計測されたトルク値と減速機の出力回転角の値を示す波形図である。(A)-(d) is a wave form diagram which shows the torque value measured by the said control system, and the value of the output rotation angle of a reduction gear. （ａ）は図１０（ｄ）に対応するシミュレーション結果を示す波形図であり、（ｂ）は図１０（ｄ）においてトルク推定の精度向上を仮定したシミュレーション結果を示す波形図である。(A) is a wave form diagram which shows the simulation result corresponding to FIG.10 (d), (b) is a wave form diagram which shows the simulation result supposing the precision improvement of torque estimation in FIG.10 (d).

以下、本発明の実施の形態について、詳細に説明する。   Hereinafter, embodiments of the present invention will be described in detail.

（概要）
まず、波動歯車減速機（ハーモニックドライブ（登録商標））の動摩擦トルク同定を行い、この動摩擦トルクを補償するトルクをフィードフォワードにより与えることで、大部分の摩擦トルクを補償する。これによって、残留摩擦を補償するために必要なトルクセンサの要求バンド幅を低減することができる。その上で、フォトインタラプタを用いた安価なトルクセンサを構成し、較正を行う。静止摩擦トルク等の補償には、このトルクセンサを使用した外乱オブザーバを構成する。以上の摩擦補償されたモータに２自由度制御を施し、位置制御されたモータに対して外乱を印加した結果、良好な特性が得られることを確認した。 (Overview)
First, the dynamic friction torque of a wave gear reducer (Harmonic Drive (registered trademark)) is identified, and the torque for compensating this dynamic friction torque is given by feedforward, so that most of the friction torque is compensated. As a result, the required bandwidth of the torque sensor required to compensate the residual friction can be reduced. Then, an inexpensive torque sensor using a photo interrupter is configured and calibrated. For compensation of static friction torque and the like, a disturbance observer using this torque sensor is configured. As a result of applying the two-degree-of-freedom control to the friction-compensated motor and applying a disturbance to the position-controlled motor, it was confirmed that good characteristics were obtained.

（実施形態）
（モータ制御装置１の構成）
図１は、実施形態に係るモータ制御装置１の構成を示すブロック図である。モータ制御装置１は、減速機３を有するモータ２を制御する。モータ２は、例えば、回転型電磁モータである。減速機３は、例えば、機械的な噛み合いを利用して高減速比で減速する歯車減速機である。歯車減速機は、歯車が所定の回転数により回転しているときに、歯車の歯先同士の接触により摩擦力が生じる。この歯車の接触による摩擦力を予め補償する電圧信号を、モータ２へ供給される電圧信号に加算することにより、減速機３の歯車による摩擦力を打ち消すことができる。 (Embodiment)
(Configuration of motor control device 1)
FIG. 1 is a block diagram illustrating a configuration of a motor control device 1 according to the embodiment. The motor control device 1 controls a motor 2 having a speed reducer 3. The motor 2 is, for example, a rotary electromagnetic motor. The reduction gear 3 is, for example, a gear reduction gear that reduces the speed with a high reduction ratio using mechanical meshing. In the gear reducer, when the gear rotates at a predetermined rotational speed, a frictional force is generated by contact between the tooth tips of the gear. By adding a voltage signal that preliminarily compensates for the frictional force due to the contact of the gear to the voltage signal supplied to the motor 2, the frictional force due to the gear of the reduction gear 3 can be canceled.

モータ制御装置１は、駆動回路８、回転速度センサ４、及び、トルクセンサ５を備えている。駆動回路８は、モータ２を駆動するための電圧信号をモータに供給する。回転速度センサ４は、減速機３の出力軸の回転速度を検出する。トルクセンサ５は、減速機３の出力軸のトルクを検出する。   The motor control device 1 includes a drive circuit 8, a rotation speed sensor 4, and a torque sensor 5. The drive circuit 8 supplies a voltage signal for driving the motor 2 to the motor. The rotational speed sensor 4 detects the rotational speed of the output shaft of the speed reducer 3. The torque sensor 5 detects the torque of the output shaft of the speed reducer 3.

モータ制御装置１には、動摩擦補償器６及び静止摩擦補償器７が設けられている。動摩擦補償器６は、回転速度センサ４により検出された減速機３の出力軸の回転速度を表す回転速度信号と、予め定められた減速機３の動摩擦モデルとに基づいて、減速機３の動摩擦トルクを補償する動摩擦補償信号を生成してモータ２に供給する。静止摩擦補償器７は、トルクセンサ５により検出された上記出力軸のトルクを表すトルク信号と、減速機３の出力軸の回転速度に対応する角加速度を表す角加速度信号に基づいて生成されるモータ慣性トルク信号とに基づいて、減速機３の静止摩擦トルクを補償する静止摩擦補償信号を生成してモータ２に供給する。   The motor control device 1 is provided with a dynamic friction compensator 6 and a static friction compensator 7. The dynamic friction compensator 6 is based on a rotational speed signal that is detected by the rotational speed sensor 4 and that represents the rotational speed of the output shaft of the speed reducer 3 and a predetermined dynamic friction model of the speed reducer 3. A dynamic friction compensation signal for compensating the torque is generated and supplied to the motor 2. The static friction compensator 7 is generated based on the torque signal representing the torque of the output shaft detected by the torque sensor 5 and the angular acceleration signal representing the angular acceleration corresponding to the rotational speed of the output shaft of the speed reducer 3. Based on the motor inertia torque signal, a static friction compensation signal for compensating the static friction torque of the speed reducer 3 is generated and supplied to the motor 2.

（フィードフォワード摩擦補償制御）
図２は、モータ制御装置１に設けられた動摩擦補償器６による摩擦補償を説明するためのブロック線図である。 (Feed forward friction compensation control)
FIG. 2 is a block diagram for explaining friction compensation by the dynamic friction compensator 6 provided in the motor control device 1.

（動摩擦特性の同定）
粘性摩擦や動摩擦は、速度の関数として比較的容易に同定することができる。減速機３の内部の動摩擦トルクを減速機３に設けられた出力軸の回転速度の関数として同定し、モータ２への入力電圧信号に重畳することで、減速機３の内部で発生する摩擦トルクの大部分を補償することができると考えられる。 (Identification of dynamic friction characteristics)
Viscous friction and dynamic friction can be identified relatively easily as a function of speed. Friction torque generated inside the speed reducer 3 by identifying the dynamic friction torque inside the speed reducer 3 as a function of the rotational speed of the output shaft provided in the speed reducer 3 and superimposing it on the input voltage signal to the motor 2 It can be assumed that most of this can be compensated.

モータ２と減速機３とから成る駆動ユニットを考える。モータ２の回路方程式は、   Consider a drive unit comprising a motor 2 and a speed reducer 3. The circuit equation of the motor 2 is

で表される。但し、θはモータ２の回転角、ｅはモータ２への印加電圧（印加電圧信号）、ｉはモータ２に流れる電流、Ｋ_Ｅはモータ２の逆起電力定数、Ｒはモータ２の端子間抵抗、Ｌはモータ２の自己インダクタンスである。モータ２の運動方程式は、 It is represented by However, the rotation angle of θ motor 2, e is the voltage applied to the motor 2 (applied voltage signal), i is the current flowing through the motor 2, K _E is the back electromotive force constant of the motor 2, R is between the motor 2 terminal Resistance, L is the self-inductance of the motor 2. The equation of motion of motor 2 is

である。但し、Ｊはモータ２の軸から見た減速機３の出力軸までのイナーシャ、γは減速機３の減速比、τ_ｍはモータ２の発生トルク、τ_ｆは減速機３の内部で生じる動摩擦トルク、τ_ｅは外部からの負荷トルクである。また、モータ２がＤＣブラシモータの場合、τ_ｍとｉはトルク定数Ｋ_Ｔを用いて、 It is. Where J is inertia from the shaft of the motor 2 to the output shaft of the speed reducer 3, γ is a reduction ratio of the speed reducer 3, τ _m is torque generated by the motor 2, and τ _f is dynamic friction generated inside the speed reducer 3. Torque, τ _e is an external load torque. Further, when the motor 2 is a DC brush motor, tau _m and i by using the torque constant _{K T,}

のように関連付けられる。自己インダクタンスＬは十分に小さいとし、さらに無負荷かつ定常状態を考えると、式（１）〜（３）は、 Are associated with each other. Assuming that the self-inductance L is sufficiently small, and further considering no-load and steady state, equations (1) to (3) are

のように整理できる。動摩擦トルクτ_ｆにおいて乾性及び粘性の影響が支配的であると仮定すると、モータ２の逆起電力と合わせ、モータ２への印加電圧ｅは回転速度ｄθ／ｄｔについての関数として与えられる。このモータ２への印加電圧ｅと回転速度ｄθ／ｄｔとの関係を実験的に求め、減速機３の動摩擦モデルを同定する。 Can be organized like Assuming the frictional torque tau _f and impact of dry and viscosity is dominant, combined with the back EMF of the motor 2, the applied voltage e of the motor 2 is given as a function of the rotation speed d [theta] / dt. The relationship between the applied voltage e to the motor 2 and the rotational speed dθ / dt is obtained experimentally, and the dynamic friction model of the speed reducer 3 is identified.

（動摩擦トルクのフィードフォワード補償）
同定された動摩擦モデルを用いて、図２に示すようなフィードフォワード補償を試みる。回転速度センサ４を構成するエンコーダの値から減速機３の出力軸の回転速度を求め、前述の動摩擦モデルから動摩擦トルクの補償に必要な電圧ｅ_ｆｆ（動摩擦補償信号）を計算する。これを制御電圧ｅ_ｃに重畳することで、減速機３の動摩擦トルクとモータ逆起電力とを併せて相殺できる。 (Feed forward compensation of dynamic friction torque)
Using the identified dynamic friction model, feedforward compensation as shown in FIG. 2 is attempted. The rotational speed of the output shaft of the speed reducer 3 is obtained from the value of the encoder constituting the rotational speed sensor 4, and the voltage _eff (dynamic friction compensation signal) necessary for compensating the dynamic friction torque is calculated from the above-mentioned dynamic friction model. By superimposing this on the control voltage e _c, it can be canceled by combining the dynamic friction torque and the motor back electromotive force of the reduction gear 3.

（外乱オブザーバ９による残留摩擦補償）
図３は、モータ制御装置１に設けられた静止摩擦補償器７による摩擦補償及び動摩擦補償器６による摩擦補償を説明するためのブロック線図である。 (Residual friction compensation by disturbance observer 9)
FIG. 3 is a block diagram for explaining friction compensation by the static friction compensator 7 and friction compensation by the dynamic friction compensator 6 provided in the motor control device 1.

（外乱オブザーバ９の設計）
ゼロ速度近傍における摩擦トルクは、静止摩擦トルクとして区別される。これは動摩擦トルクと異なり、何らかの値の関数とした摩擦モデルを仮定することが難しい。そこでトルクセンサ５を使用した外乱オブザーバ９を構成し、静止摩擦トルクを補償することを考える。 (Design of disturbance observer 9)
Friction torque near zero speed is distinguished as static friction torque. Unlike dynamic friction torque, it is difficult to assume a friction model as a function of some value. Therefore, it is considered that a disturbance observer 9 using the torque sensor 5 is configured to compensate for the static friction torque.

仮に、図１及び図２で説明した動摩擦補償器６により、すべての摩擦トルクが補償されたとすると、粘性抵抗が補償された減速機３付きモータ２の振る舞いは、理論モデル   If all the friction torques are compensated by the dynamic friction compensator 6 described with reference to FIGS. 1 and 2, the behavior of the motor 2 with the speed reducer 3 compensated for the viscous resistance is a theoretical model.

に従うはずである。ここで、入力電圧をｅ_ｕとしている。 Should follow. Here, we have a e _u the input voltage.

しかしながら、実際には非線形摩擦トルクやモデル化誤差が残り、理論通りに減速機３付きモータ２が振る舞うことは無い。   However, in practice, nonlinear friction torque and modeling error remain, and the motor 2 with the speed reducer 3 does not behave as theoretically.

これらの非線形摩擦トルク、モデル化誤差を含む外乱の電圧換算値を、   These nonlinear friction torques, disturbance voltage conversion values including modeling errors,

で表すと、減速機３付きモータ２の運動方程式は、 The equation of motion of the motor 2 with the speed reducer 3 is

となる。そこで、図３に示すような外乱オブザーバ９を用い、応答の理想化を試みる。但し、トルクセンサ５の出力電圧特性をｈ（τ）、その逆関数をｈ⁻¹（ｅ）とし、各添字ｎはノミナル値であることを表す。 It becomes. Therefore, a disturbance observer 9 as shown in FIG. 3 is used to attempt an ideal response. However, the output voltage characteristic of the torque sensor 5 is h (τ), its inverse function is h ⁻¹ (e), and each subscript n represents a nominal value.

外乱オブザーバ９は、外乱に対応するトルク信号が入力される外乱トルク推定器１０が与える負荷トルク推定値（トルク推定信号）τ_ｅｅと、回転速度センサ４により検出された減速機３の出力軸の回転速度から計算される回転角加速度ｄθ_Ｍ／ｄｔ（角加速度信号）に基づいて生成されるモータ慣性トルク信号とから、減速機３付きモータ２の出力トルクを計算する。そして、この出力トルクを外乱オブザーバ９の換算要素１６が電圧に換算し、入力電圧ｅ_ｕに対応する印加電圧信号との差分を取ることで、残留摩擦トルクの推定電圧換算値を表す信号 The disturbance observer 9 includes a load torque estimated value (torque estimation signal) τ _ee given by a disturbance torque estimator 10 to which a torque signal corresponding to the disturbance is input, and an output shaft of the speed reducer 3 detected by the rotational speed sensor 4. The output torque of the motor 2 with the speed reducer 3 is calculated from the motor inertia torque signal generated based on the rotational angular acceleration dθ _M / dt (angular acceleration signal) calculated from the rotational speed. Then, the output torque is converted to a converted element 16 the voltage of the disturbance observer 9, by taking the difference between the applied voltage signal corresponding to the input voltage e _u, the signal representative of the estimated voltage conversion value of the residual friction torque

を得る。次に、この残留摩擦トルクの推定電圧換算値を表す信号Δｅ_ｆｆ（静止摩擦補償信号）を二次遅れ要素１７を通して安定化し、制御電圧ｅ_ｃに加算して入力電圧ｅ_ｕにフィードバックすることで、 Get. Next, by feeding back the residual friction torque estimated voltage conversion value signal representative of the .DELTA.e _{ff (static} friction compensation signal) is stabilized through the secondary delay element 17, the input voltage e _u is added to the control voltage e _c ,

とし、式（５）のモデルにより近い応答を実現できる。 And a response closer to the model of Equation (5) can be realized.

（安価なトルク計測システム）
従来開発されてきたシステムの多くは、外乱オブザーバ９を用いた加速度ベースの制御により理想的な応答を実現している。しかしながら、すべての外乱を外乱オブザーバ９により補償しようとすると、バンド幅の大きいトルクセンサが必要となり、システムが高価になる。そこで、図２では、速度参照によるモデル化可能な動摩擦トルクのフィードフォワード補償について述べた。図１及び図３に示すように、動摩擦補償器６による動摩擦トルクのフィードフォワード補償を、静摩擦補償器７による静摩擦トルクのフィードバック補償と併用することにより、バンド幅の小さい安価なトルクセンサ５でも比較的精度よく外乱補償を行うことが可能である。 (Inexpensive torque measurement system)
Many systems that have been developed in the past achieve an ideal response by acceleration-based control using the disturbance observer 9. However, if all disturbances are to be compensated by the disturbance observer 9, a torque sensor with a large bandwidth is required, and the system becomes expensive. Therefore, FIG. 2 describes the feedforward compensation of the dynamic friction torque that can be modeled by the speed reference. As shown in FIGS. 1 and 3, by using the feedforward compensation of the dynamic friction torque by the dynamic friction compensator 6 together with the feedback compensation of the static friction torque by the static friction compensator 7, even an inexpensive torque sensor 5 having a small bandwidth can be compared. It is possible to perform disturbance compensation with high accuracy.

図４（ａ）（ｂ）はモータ制御装置１の実験装置に設けられたモータ２及び減速機３の構成を示す斜視図である。図５は、モータ制御装置１の実験装置に設けられたトルクセンサ５の構成を示す斜視図である。図６（ａ）（ｂ）はトルクセンサ５の動作を説明するための図である。   4A and 4B are perspective views showing configurations of the motor 2 and the speed reducer 3 provided in the experimental device of the motor control device 1. FIG. FIG. 5 is a perspective view showing the configuration of the torque sensor 5 provided in the experimental device of the motor control device 1. 6A and 6B are diagrams for explaining the operation of the torque sensor 5.

モータ２及び減速機３は、基板上に平行に設けられている。モータ２の軸と減速機３の入力軸とは、タイミングベルト１１によって結合されている。減速機３の出力軸にはアーム１２が取り付けられている。アーム１２と減速機３との間に、リング１４が固定されている。リング１４にはスリット１５が形成されている。アーム１２のスリット１５に対応する位置に、トルクセンサ５を構成するフォトインタラプタ１３が配置されている。   The motor 2 and the speed reducer 3 are provided in parallel on the substrate. The shaft of the motor 2 and the input shaft of the speed reducer 3 are coupled by a timing belt 11. An arm 12 is attached to the output shaft of the speed reducer 3. A ring 14 is fixed between the arm 12 and the speed reducer 3. A slit 15 is formed in the ring 14. A photo interrupter 13 constituting the torque sensor 5 is arranged at a position corresponding to the slit 15 of the arm 12.

本実施形態では，フォトインタラプタ１３を備えたトルクセンサ５を採用する。フォトインタラプタ１３は、発光ダイオードとフォトトランジスタとの間を物体が遮ることで生じる光量変化を、出力電圧の変化として検出する。   In this embodiment, the torque sensor 5 provided with the photo interrupter 13 is employed. The photo interrupter 13 detects a change in light amount caused by an object blocking between the light emitting diode and the phototransistor as a change in output voltage.

図５及び図６に示すように、アーム１２にフォトインタラプタ１３を設け、リング１４にスリット１５を形成する。アーム１２にトルクを負荷すると曲げ変形によりフォトインタラプタ１３とスリット１５との間に相対的な変位が生じ、スリット１５を通過する光量が変化するため、フォトインタラプタ１３の原理をトルク計測に応用できる。   As shown in FIGS. 5 and 6, a photo interrupter 13 is provided on the arm 12, and a slit 15 is formed on the ring 14. When a torque is applied to the arm 12, a relative displacement occurs between the photo interrupter 13 and the slit 15 due to bending deformation, and the amount of light passing through the slit 15 changes. Therefore, the principle of the photo interrupter 13 can be applied to torque measurement.

フォトインタラプタ１３を用いたトルクセンサ５は、出力が非線形であり、温度に高感度であるという点を除けば、小型軽量であり、なおかつ安価であるというメリットを有する。前述した外乱オブザーバ９で精度よく外乱を補償するためには、このトルクセンサ５の特性を実験によって同定し、トルクセンサ５から出力された電圧に基づいて、実際に負荷されたトルクを正しく推定するための較正が重要となる。   The torque sensor 5 using the photo interrupter 13 has the merit of being small and light and inexpensive, except that the output is nonlinear and the temperature is highly sensitive. In order to compensate for the disturbance with the above-described disturbance observer 9 accurately, the characteristics of the torque sensor 5 are identified by experiments, and the actually applied torque is correctly estimated based on the voltage output from the torque sensor 5. Calibration is important.

（実験）
（動摩擦トルク・粘性抵抗の同定実験）
モータ２(maxon RE-max 29)と減速機３（ハーモニックドライブ（登録商標）減速機(SHG-14-80-2UH-SP)、波動歯車減速機）から成る図４（ａ）（ｂ）に示す駆動ユニットを用いて、式（４）に基づきモータ２が回転する時に減速機３に発生する動摩擦トルクを計測する実験を行った。 (Experiment)
(Identification experiment of dynamic friction torque and viscous resistance)
4 (a) and 4 (b) comprising a motor 2 (maxon RE-max 29) and a reduction gear 3 (harmonic drive (registered trademark) reduction gear (SHG-14-80-2UH-SP), wave gear reduction gear). Using the drive unit shown, an experiment was performed to measure the dynamic friction torque generated in the speed reducer 3 when the motor 2 rotates based on the equation (4).

モータ２はマイコンを含む駆動回路８から制御するため、電圧はＰＷＭ（Pulse Width Modulation）で与え、デューティ比で調整した。実験はデューティ比の増減による回転速度の変化を計測することで行い、デューティ比は静止状態では１．０％、回転状態では０．２％ずつ、最大±６４．０％まで増減させた。但し、デューティ比の符号はモータ２への印加電圧の正負に対応する。回転速度は、デューティ比を１％ないし０．２％増減させるごとに、周期２[ｍｓ]で１０００サンプル計測し、これらの平均を取った。このときの結果を図７ に示す。   Since the motor 2 is controlled from the drive circuit 8 including a microcomputer, the voltage is given by PWM (Pulse Width Modulation) and adjusted by the duty ratio. The experiment was performed by measuring the change in the rotation speed due to the increase / decrease of the duty ratio, and the duty ratio was increased / decreased by 1.0% in the stationary state and 0.2% in the rotational state, up to ± 64.0%. However, the sign of the duty ratio corresponds to the sign of the voltage applied to the motor 2. The rotational speed was measured by taking 1000 samples at a period of 2 [ms] every time the duty ratio was increased or decreased by 1% to 0.2%, and taking the average. The result at this time is shown in FIG.

図７は、モータ制御装置１に設けられた減速機３の出力軸の角速度と上記出力軸のトルクを表すＰＷＭデューティ比との関係を示すグラフである。スイッチング素子であるＭＯＳ ＦＥＴ（Metal-Oxide-Semiconductor Field-Effect Transistor）のオン抵抗温度特性により温度ドリフトが生じているが、回転速度について粘性特性と符号関数である乾性特性の和となっていることが図から読み取れる。     FIG. 7 is a graph showing the relationship between the angular speed of the output shaft of the speed reducer 3 provided in the motor control device 1 and the PWM duty ratio representing the torque of the output shaft. Temperature drift occurs due to the on-resistance temperature characteristics of the MOS FET (Metal-Oxide-Semiconductor Field-Effect Transistor), which is a switching element, but the rotational speed is the sum of the viscosity characteristics and the dryness characteristics that are sign functions. Can be read from the figure.

（トルクセンサ５の特性確認）
図８は、トルクセンサ５の出力電圧と減速機３の出力軸のトルクとの間の関係を示すグラフである。図４（ａ）に示す実験機に着脱可能なアーム１２を図４（ｂ）に示すように取り付け、アーム１２へ負荷するトルクとトルクセンサ５の出力電圧との間の関係を実験によって計測した、この結果が図８に示すグラフであり、無視できない大きさのヒステリシスを有する曲線Ｃが確認された。このヒステリシスはアーム１２の材料の粘性と塑性変形とが大きな原因であると考えられる。また、温度変化等による大きなドリフトも確認された。本来ならば、これらのヒステリシスやドリフトは適当なモデルとパラメータの同定とにより補償される方が望ましい。しかしながら、本実施形態ではトルクセンサ５を構成するフォトインタラプタ１３の出力電圧と減速機３の出力軸のトルクとの間に図８に示す直線Ｌのような比例関係が成り立つことを仮定し、精度の低いトルク推定下での制御性能を評価する。 (Characteristic confirmation of torque sensor 5)
FIG. 8 is a graph showing the relationship between the output voltage of the torque sensor 5 and the torque of the output shaft of the speed reducer 3. A removable arm 12 is attached to the experimental machine shown in FIG. 4A as shown in FIG. 4B, and the relationship between the torque applied to the arm 12 and the output voltage of the torque sensor 5 is measured by experiment. This result is the graph shown in FIG. 8, and a curve C having a hysteresis that cannot be ignored was confirmed. This hysteresis is considered to be largely caused by the viscosity and plastic deformation of the material of the arm 12. A large drift due to temperature change was also confirmed. Originally, it is desirable that these hysteresis and drift are compensated by appropriate model and parameter identification. However, in this embodiment, it is assumed that a proportional relationship such as a straight line L shown in FIG. 8 is established between the output voltage of the photo interrupter 13 constituting the torque sensor 5 and the torque of the output shaft of the speed reducer 3. The control performance under low torque estimation is evaluated.

（制御実験）
図３に示したモータ制御装置１に対して位置制御実験を行った。実機のパラメータは、
Ｒ_ｎ＝２．３６〔Ω〕、
Ｋ_Ｔｎ＝２．５８×１０−２〔Ｎ・ｍ／Ａ〕、
Ｊ_ｎ＝７．０１×１０−６〔ｋｇ・ｍ^２〕、
γ＝１２０、
ω_Ｌ＝２５０〔ｒａｄ／ｓ〕、
である。 (Control experiment)
A position control experiment was performed on the motor control device 1 shown in FIG. The actual parameters are
R _n = 2.36 [Ω],
K _Tn = 2.58 × 10-2 [N · m / A],
J _n = 7.01 × 10 −6 [kg · m ² ],
γ = 120,
ω _L = 250 [rad / s],
It is.

図７に示すように同定されたハーモニックドライブ（登録商標）式の減速機３の動摩擦トルクに対して、フィードフォワード摩擦補償を施す場合は、制御が不安定にならないように、この動摩擦トルクよりも５〜１０％小さめの補償トルクを電圧換算して重畳した。上記同定された動摩擦トルクと上記小さめの補償トルクとの間の差分のトルクは、静止摩擦補償器７によるフィードバック制御により補償される。   When feedforward friction compensation is applied to the dynamic friction torque of the harmonic drive (registered trademark) type reduction gear 3 identified as shown in FIG. 7, the dynamic friction torque is controlled to prevent the control from becoming unstable. 5-10% smaller compensation torque was converted into voltage and superimposed. The torque difference between the identified dynamic friction torque and the smaller compensation torque is compensated by feedback control by the static friction compensator 7.

このフィードフォワード摩擦補償で補償できなかった摩擦トルクは、外乱オブザーバ９により補償される。摩擦トルクが完全に補償された理想的なモータの入力電圧ｅ_ｕから出力回転角θまでの伝達関数は、 Friction torque that could not be compensated by this feedforward friction compensation is compensated by the disturbance observer 9. The transfer function from the ideal motor input voltage _eu to the output rotation angle θ with the friction torque fully compensated is

で表される。これに対し、ＰＩＤ制御器 It is represented by In contrast, PID controller

と、目標値応答を改善するためのフィードフォワード制御器 And a feedforward controller to improve the target value response

から成る２自由度制御器を設計し、図９に示すような制御システムを構成した。 A two-degree-of-freedom controller consisting of the above was designed to constitute a control system as shown in FIG.

但し、Ｋ_ＰはＰＩＤ制御の比例ゲインであり、Ｋ_Ｉはその積分ゲインであり、Ｋ_Ｄはその微分ゲインである。ω_ｎ、αω_ｎはフィードバック系の固有振動数であり、ζは減衰係数であり、これらには、 However, _{K P} is a proportional gain of PID control, _{K I} is the integral gain, _{K D} is its differential gain. ω _n and αω _n are the natural frequencies of the feedback system, ζ is the damping coefficient,

の関係がある。ω′_ｎ、αω′_ｎは目標値応答に関する固有振動数であり、大きめにとることで速応性の高い目標値応答を可能にする。 There is a relationship. ω ′ _n and αω ′ _n are natural frequencies related to the target value response, and by taking a large value, it is possible to achieve a target value response with high responsiveness.

以上のシステムに対して、外乱応答を考慮した小さめのＰＩＤゲインを設定し、制御周期２〔ｍｓ〕の２自由度制御実験を行った。各パラメータは、
ω_ｎ＝４．５０〔ｒａｄ／ｓ〕、
ω′_ｎ＝５０．０〔ｒａｄ／ｓ〕、
ζ＝０．６０、
α＝０．８０、
とした。 A small PID gain considering the disturbance response was set for the above system, and a two-degree-of-freedom control experiment with a control period of 2 [ms] was performed. Each parameter is
ω _n = 4.50 [rad / s],
ω ′ _n = 50.0 [rad / s],
ζ = 0.60,
α = 0.80,
It was.

実験は、
実験（ａ）補償なし、
実験（ｂ）フィードフォワード補償のみ、
実験（ｃ）外乱オブザーバのみ、
実験（ｄ）フィードフォワード補償と外乱オブザーバとの併用
の４通りを実施した。各実験において、下記の４つの操作を与えた。
１．減速機３の出力軸の角度が１０．０°となる目標値を設定する。
２．ゆっくりと外乱トルクを負荷する。
３．外乱トルクを除荷する。
４．素早く瞬間的に外乱トルクを負荷する。 The experiment
Experiment (a) No compensation,
Experiment (b) Feedforward compensation only,
Experiment (c) Disturbance observer only
Experiment (d) Four combinations of feed forward compensation and disturbance observer were used. In each experiment, the following four operations were given.
1. A target value is set so that the angle of the output shaft of the speed reducer 3 is 10.0 °.
2. Slowly apply disturbance torque.
3. Unload disturbance torque.
4). Load disturbance torque quickly and instantaneously.

計測トルクの値と減速機３の出力回転角（ジョイント回転角）の値をグラフにしたものを図１０（ａ）〜（ｄ）に示す。計測トルクには、ＰＷＭ周波数や電源の商用周波数由来のノイズが多く載っており、ヒステリシスやドリフトも見られる。目標値や外乱トルクに対する出力角度の応答を比較する。   10A to 10D are graphs showing the measured torque value and the output rotation angle (joint rotation angle) value of the speed reducer 3. The measured torque contains a lot of noise derived from the PWM frequency and the commercial frequency of the power supply, and hysteresis and drift are also seen. Compare response of output angle to target value and disturbance torque.

図１０（ａ）を参照すると、補償なしでは摩擦トルクが非常に大きく、ジョイント回転角は、目標値にも外乱トルクにも全く応答できていない。本実施形態で設定されたＰＩＤゲインが非常に小さいということは、この条件における目標値応答がよく表している。   Referring to FIG. 10A, the friction torque is very large without compensation, and the joint rotation angle cannot respond at all to the target value and the disturbance torque. That the PID gain set in the present embodiment is very small well represents the target value response under this condition.

図１０（ｂ）を参照すると、フィードフォワード補償（動摩擦補償器６）のみの場合は、ある程度応答が改善されているが、残留摩擦により停止してしまい、これによるフィードバック制御の積分器の発散によりジョイント回転角の挙動が振動的となっている。   Referring to FIG. 10 (b), in the case of only feedforward compensation (dynamic friction compensator 6), the response is improved to some extent, but it stops due to residual friction, and due to the divergence of the integrator of feedback control due to this, The behavior of the joint rotation angle is oscillatory.

図１０（ｃ）を参照すると、外乱オブザーバ９（静止摩擦補償器７）のみの場合は、目標値応答、外乱応答ともに大きく改善されている。しかしながら、目標値近傍で振動的となっていることが分かる。これは、外乱オブザーバ９の安定化のために挿入している２次遅れ要素１７により、補償に遅延が生じるためと考えられる。   Referring to FIG. 10C, in the case of only the disturbance observer 9 (static friction compensator 7), both the target value response and the disturbance response are greatly improved. However, it can be seen that it is oscillating near the target value. This is presumably because a delay occurs in compensation due to the second-order delay element 17 inserted for stabilizing the disturbance observer 9.

図１０（ｄ）を参照すると、フィードフォワード補償（動摩擦補償器６）と外乱オブザーバ９（静止摩擦補償器７）との併用によって、外乱オブザーバ９の補償量を低減することができ、図１０（ｃ）で現れていた遅延を解消することができる。しかしながら、この場合でも目標値近傍で振動が生じ、完全に収束させることはできていない。   Referring to FIG. 10D, the combined use of the feedforward compensation (dynamic friction compensator 6) and the disturbance observer 9 (static friction compensator 7) can reduce the compensation amount of the disturbance observer 9, and FIG. The delay that appeared in c) can be eliminated. However, even in this case, vibration occurs in the vicinity of the target value, and complete convergence cannot be achieved.

この原因について、シミュレーションを用いて考察した。考えられる原因としては、タイミングベルト１１の粘弾性を無視した制御器を構成していること、トルクセンサ５のトルク推定精度が低いこと、制御周期が十分でなく、外乱オブザーバ９の残留摩擦補償が遅れること等が考えられる。これらを検証するため、タイミングベルト１１の粘弾性を考慮した２慣性系   This cause was considered using simulation. Possible causes are that a controller that ignores the viscoelasticity of the timing belt 11 is configured, the torque estimation accuracy of the torque sensor 5 is low, the control cycle is not sufficient, and the residual friction compensation of the disturbance observer 9 is It can be delayed. In order to verify these, a two-inertia system considering the viscoelasticity of the timing belt 11

として実機のモデル化を行った。但し、
Ｊ_ｍはモータ２の出力軸のイナーシャ、
θ_ｍはモータ２の回転角、
Ｊ_ｇは減速機３の入力軸のイナーシャ、
θ_ｇは減速機３の入力軸の回転角、
である。
また、
τ_ｇは減速機３の入力軸に作用するトルクであり、
タイミングベルト１１の弾性係数をＫ_ｂ、粘性係数をＣ_ｂとすると、 As an actual model. However,
J _m is the inertia of the output shaft of the motor 2,
θ _m is the rotation angle of the motor 2,
_Jg is the inertia of the input shaft of the reducer 3,
θ _g is the rotation angle of the input shaft of the speed reducer 3,
It is.
Also,
τ _g is a torque acting on the input shaft of the speed reducer 3,
When the elastic coefficient of the timing belt 11 is K _b and the viscosity coefficient is C _b ,

と表される。予備実験より、Ｋ_ｂ≒１０〔Ｎ・ｍ／ｒａｄ〕、Ｃ_ｂ≒０．０１〔Ｎ・ｍ・ｓ／ｒａｄ〕であることが分かっているが、これよりも弾性を大きくし、１慣性系に近似できる場合でもシミュレーション結果は変わらなかった。従って、振動はタイミングベルト１１に起因するものではないといえる。 It is expressed. From preliminary experiments, it is known that K _b ≈10 [N · m / rad] and C _b ≈0.01 [N · m · s / rad]. The simulation results did not change even when the system could be approximated. Therefore, it can be said that the vibration is not caused by the timing belt 11.

一方、本実施形態において制御周期は２〔ｍｓ〕としたが、この制御周期をより小さく設定することで、外乱オブザーバ９の安定化のためのＬＰＦ（Low Pass Filter）のカットオフ周波数ω_Ｌを大きく取れることが分かった。即ち、これにより補償の遅れを低減し残留摩擦の影響を小さくすることで、滑らかな制御を実現することができると考えられる。さらに、微小な振動を除去するためには、今後ヒステリシスやドリフトの対策を含めたトルク推定精度の向上を図る必要があると分かった。 On the other hand, in the present embodiment, the control cycle is 2 [ms], but by setting this control cycle smaller, the cutoff frequency ω _L of the LPF (Low Pass Filter) for stabilizing the disturbance observer 9 is set. I found that I could take it big. That is, it is considered that smooth control can be realized by reducing the delay of compensation and reducing the influence of residual friction. Furthermore, in order to remove minute vibrations, it was found that it is necessary to improve the accuracy of torque estimation including countermeasures for hysteresis and drift.

以上を示したものが図１１である。図１１（ａ）は図１０（ｄ）に対応するシミュレーション結果を示す波形図であり、（ｂ）は図１０（ｄ）においてトルク推定の精度向上を仮定したシミュレーション結果を示す波形図である。実験（ｄ）のシミュレーションを示す波形図を示すのが図１１（ａ）であり、実験（ｄ）において制御周期を５００〔μｓ〕、ＬＰＦのカットオフ周波数をω_Ｌ＝５００〔ｒａｄ／ｓ〕とし、トルク推定の精度向上（ここではノイズの軽減）を仮定したシミュレーション結果を表す波形図を示すのが図１１（ｂ）である。 FIG. 11 shows the above. FIG. 11A is a waveform diagram showing a simulation result corresponding to FIG. 10D, and FIG. 11B is a waveform diagram showing a simulation result assuming an improvement in torque estimation accuracy in FIG. 10D. FIG. 11A shows a waveform diagram showing the simulation of the experiment (d). In the experiment (d), the control period is 500 [μs], and the cutoff frequency of the LPF is ω _L = 500 [rad / s]. FIG. 11B shows a waveform diagram representing a simulation result assuming an improvement in accuracy of torque estimation (here, reduction of noise).

（変形例）
上述した実施形態では、機械的な噛み合いを利用して高減速比で減速する歯車減速機によりモータ２の回転を減速する例を示した。しかしながら、本発明はこれに限定されない。例えば、油圧機構によりモータ２の回転を減速するように構成しても良い。この場合、動摩擦補償器６は、配管の壁面と油との間で生じる粘性摩擦を補償する。 (Modification)
In the above-described embodiment, the example in which the rotation of the motor 2 is decelerated by the gear reduction device that decelerates at a high reduction ratio using mechanical meshing has been described. However, the present invention is not limited to this. For example, the rotation of the motor 2 may be decelerated by a hydraulic mechanism. In this case, the dynamic friction compensator 6 compensates for the viscous friction generated between the wall surface of the pipe and the oil.

また、実施形態では、モータ２が、回転型電磁モータである例を示したが、本発明はこれに限定されない。例えば、モータ２はリニアモータであってもよい。   In the embodiment, the motor 2 is a rotary electromagnetic motor. However, the present invention is not limited to this. For example, the motor 2 may be a linear motor.

さらに、実施形態では、減速機３が波動歯車減速機である例を示したが、本発明はこれに限定されない。減速機３は、遊星歯車減速機等の、波動歯車減速機以外の歯車減速機であってもよい。   Furthermore, in the embodiment, an example in which the speed reducer 3 is a wave gear speed reducer has been described, but the present invention is not limited to this. The reducer 3 may be a gear reducer other than a wave gear reducer, such as a planetary gear reducer.

（実施形態のまとめ）
本実施形態では、回転型電磁方式のモータ２と高減速比の減速機３との組み合わせにおいて、ソフトウェア的に柔軟かつ高精度な関節制御を、安価なハードウェア構成で実現した。その上で、外乱オブザーバ９とフィードフォワード摩擦補償とを併用することでトルクセンサ５の要求バンド幅を低減し、安価に比較的精度よく外乱補償を行う方法を提案した。また、外乱存在下で位置制御実験を実施することで、提案方法により、大きな摩擦を持つ駆動システムでも応答の改善が可能であることを示した。 (Summary of embodiment)
In the present embodiment, in the combination of the rotary electromagnetic motor 2 and the reduction gear 3 with a high reduction ratio, soft and highly accurate joint control in software is realized with an inexpensive hardware configuration. In addition, a method for reducing the required bandwidth of the torque sensor 5 by using both the disturbance observer 9 and feedforward friction compensation and performing disturbance compensation at a low cost and with relatively high accuracy has been proposed. In addition, by conducting a position control experiment in the presence of disturbances, it was shown that the proposed method can improve the response even in a drive system with large friction.

ロボットの柔らかな制御を阻む原因は、主に、減速機３の内部で生じる摩擦トルクである。従来の構成において外乱を計測してフィードバックすることは、結局、この減速機３の内部で生じる摩擦トルクを推定して補償していることと同義である。本実施形態は、事前に、減速機３の内部で生じる摩擦トルクを同定しておき、フィードフォワードに上記摩擦トルクを補償することにより、フィードバックによって補償しなければならない摩擦トルクを減少させる。この結果、外乱を測定するトルクセンサ５に要求される精度が低減する。   The cause of hindering the soft control of the robot is mainly the friction torque generated inside the speed reducer 3. Measuring and feeding back the disturbance in the conventional configuration is equivalent to estimating and compensating for the friction torque generated inside the speed reducer 3 after all. In the present embodiment, the friction torque generated inside the reduction gear 3 is identified in advance, and the friction torque that must be compensated by feedback is reduced by compensating the friction torque in feed forward. As a result, the accuracy required for the torque sensor 5 that measures the disturbance is reduced.

具体的には、従来の構成において必要とされるトルクセンサの単価が数十万円から百万円程度であったが、本実施形態によって数百円程度に減少する。また、本実施形態によれば、制御精度を著しく低下させるバネ等の柔軟な機械要素、空気圧アクチュエータ等の使用が不要になる上に、ソフトウェアにより摩擦トルクの補償程度を調節することができる。従って、作業中に自由に位置決め精度を回復することが可能になる。   Specifically, the unit price of the torque sensor required in the conventional configuration is about several hundred thousand yen to about one million yen, but this embodiment reduces it to about several hundred yen. Further, according to the present embodiment, it is not necessary to use a flexible mechanical element such as a spring or a pneumatic actuator that significantly reduces the control accuracy, and the degree of compensation of the friction torque can be adjusted by software. Therefore, the positioning accuracy can be freely recovered during the work.

歯車式の減速機３のような回転型減速機の内部摩擦は、回転速度のみの関数となり、負荷慣性、回転角度に依存しない。従って、ロボットに組み込む前のモータ２、減速機３を含むユニット単体の段階で、減速機３の内部で生じる摩擦トルクの特性を同定することが可能である。減速機３の回転速度はエンコーダによる回転速度センサ４により比較的容易に計測することができる。このため、計測した回転速度に基づいて、減速機３の内部摩擦を打ち消すフィードフォワード信号を予めモータ２への入力電圧信号に重畳することが可能である。   The internal friction of a rotary speed reducer such as the gear type speed reducer 3 is a function of only the rotational speed and does not depend on the load inertia and the rotation angle. Therefore, it is possible to identify the characteristics of the friction torque generated inside the speed reducer 3 at the stage of a single unit including the motor 2 and the speed reducer 3 before being incorporated into the robot. The rotational speed of the speed reducer 3 can be measured relatively easily by the rotational speed sensor 4 using an encoder. For this reason, it is possible to superimpose a feedforward signal for canceling out the internal friction of the speed reducer 3 on the input voltage signal to the motor 2 in advance based on the measured rotational speed.

停動トルクが減速機３に作用した時に発生する静止摩擦トルクは推定することができないので、上記静止摩擦トルクを補償するためには、これをトルクセンサ５によって計測する必要があるが、要求される計測精度はそれほど高くないので、トルクセンサ５は安価なセンサで十分実用に足りる。   Since the static friction torque generated when the stationary torque acts on the reduction gear 3 cannot be estimated, in order to compensate for the static friction torque, it is necessary to measure this with the torque sensor 5, but this is required. Since the measurement accuracy is not so high, the torque sensor 5 is an inexpensive sensor and is sufficiently practical.

以上のように本実施形態によれば、フィードバック制御とフィードフォワード制御との組み合わせによって、柔らかさと硬さとを兼ね備えたモータ制御を極めて安価に実現することができる。本実施形態は、従来のロボットが苦手としてきた作業、即ち、物や人との頻繁且つ不規則な衝突・接触を伴う作業の自動化を大きく促進し、新産業の創出にもつながる可能性が高い。   As described above, according to the present embodiment, motor control having both softness and hardness can be realized at a very low cost by combining feedback control and feedforward control. This embodiment greatly facilitates the automation of work that conventional robots have not been good at, that is, work that involves frequent and irregular collisions and contact with objects and people, and is likely to lead to the creation of new industries. .

〔ソフトウェアによる実現例〕
モータ制御装置１の制御ブロック（特に動摩擦補償器６及び静止摩擦補償器７）は、集積回路（ＩＣチップ）等に形成された論理回路（ハードウェア）によって実現してもよいし、ＣＰＵ（Central Processing Unit）を用いてソフトウェアによって実現してもよい。 [Example of software implementation]
The control blocks (particularly the dynamic friction compensator 6 and the static friction compensator 7) of the motor control device 1 may be realized by a logic circuit (hardware) formed in an integrated circuit (IC chip) or the like, or a CPU (Central It may be realized by software using a Processing Unit.

後者の場合、動摩擦補償器６及び静止摩擦補償器７は、各機能を実現するソフトウェアであるプログラムの命令を実行するＣＰＵ、上記プログラム及び各種データがコンピュータ（又はＣＰＵ）で読み取り可能に記録されたＲＯＭ（Read Only Memory）又は記憶装置（これらを「記録媒体」と称する）、上記プログラムを展開するＲＡＭ（Random Access Memory）などを備えている。そして、コンピュータ（又はＣＰＵ）が上記プログラムを上記記録媒体から読み取って実行することにより、本発明の目的が達成される。上記記録媒体としては、「一時的でない有形の媒体」、例えば、テープ、ディスク、カード、半導体メモリ、プログラマブルな論理回路などを用いることができる。また、上記プログラムは、該プログラムを伝送可能な任意の伝送媒体（通信ネットワークや放送波等）を介して上記コンピュータに供給されてもよい。なお、本発明は、上記プログラムが電子的な伝送によって具現化された、搬送波に埋め込まれたデータ信号の形態でも実現され得る。   In the latter case, the dynamic friction compensator 6 and the static friction compensator 7 are recorded with a CPU that executes instructions of a program that is software that realizes each function, and the program and various data that can be read by a computer (or CPU). A ROM (Read Only Memory) or a storage device (these are referred to as “recording media”), a RAM (Random Access Memory) for expanding the program, and the like are provided. And the objective of this invention is achieved when a computer (or CPU) reads the said program from the said recording medium and runs it. As the recording medium, a “non-temporary tangible medium” such as a tape, a disk, a card, a semiconductor memory, a programmable logic circuit, or the like can be used. The program may be supplied to the computer via an arbitrary transmission medium (such as a communication network or a broadcast wave) that can transmit the program. The present invention can also be realized in the form of a data signal embedded in a carrier wave in which the program is embodied by electronic transmission.

本発明は上述した各実施形態に限定されるものではなく、請求項に示した範囲で種々の変更が可能であり、異なる実施形態にそれぞれ開示された技術的手段を適宜組み合わせて得られる実施形態についても本発明の技術的範囲に含まれる。   The present invention is not limited to the above-described embodiments, and various modifications are possible within the scope shown in the claims, and embodiments obtained by appropriately combining technical means disclosed in different embodiments. Is also included in the technical scope of the present invention.

本発明は、減速機を有するモータを制御するモータ制御装置及びモータ制御方法に利用することができる。   The present invention can be used for a motor control device and a motor control method for controlling a motor having a reduction gear.

また、本発明に係るモータ制御装置及びモータ制御方法は、工作機械、ロボットに利用することができ、特に、対象とぶつかることによってはじめて任務が遂行される作業をするロボット、例えば、介護ロボット、被災地の復興支援ロボット、災害救助ロボット等に利用することができる。   The motor control device and motor control method according to the present invention can be used for machine tools and robots, and in particular, robots that perform tasks for the first time when they collide with objects, such as nursing robots, disasters, etc. It can be used for local reconstruction support robots and disaster rescue robots.

さらに本発明は、セルベース生産システムにおける多種の複雑形状部品のロボットによる組立作業に利用することができる。   Furthermore, the present invention can be used for assembly work by a robot of various complex shaped parts in a cell-based production system.

さらに本発明は、高齢者、障碍者の生活支援、福祉介護支援に利用することができる。   Furthermore, the present invention can be used for life support and welfare care support for the elderly and persons with disabilities.

さらに本発明は、原子力発電所のような超複雑環境内での高度な機器操作を含む遠隔ロボット作業に利用することができる。   Furthermore, the present invention can be used for remote robot work including advanced equipment operation in a super-complex environment such as a nuclear power plant.

１ モータ制御装置
２ モータ
３ 減速機
４ 回転速度センサ
５ トルクセンサ
６ 動摩擦補償器
７ 静止摩擦補償器
８ 駆動回路
９ 外乱オブザーバ
１０ トルク推定器
１１ タイミングベルト
１２ アーム
１３ フォトインタラプタ
１４ リング
１５ スリット DESCRIPTION OF SYMBOLS 1 Motor controller 2 Motor 3 Reducer 4 Rotation speed sensor 5 Torque sensor 6 Dynamic friction compensator 7 Static friction compensator 8 Drive circuit 9 Disturbance observer 10 Torque estimator 11 Timing belt 12 Arm 13 Photo interrupter 14 Ring 15 Slit

Claims (6)

減速機を有するモータを制御するモータ制御装置であって、
前記減速機の出力軸の回転速度を検出する回転速度センサと、
前記減速機の出力軸のトルクを検出するトルクセンサと、
前記回転速度センサにより検出された前記回転速度を表す回転速度信号と、予め定められた前記減速機の動摩擦モデルとに基づいて、前記減速機の動摩擦トルクを補償する動摩擦補償信号を生成して前記モータに供給する動摩擦補償器と、
前記トルクセンサにより検出された前記トルクを表すトルク信号と、前記減速機の出力軸の回転速度に対応する角加速度を表す角加速度信号に基づいて生成されるモータ慣性トルク信号とに基づいて、前記減速機の静止摩擦トルクを補償する静止摩擦補償信号を生成して前記モータに供給する静止摩擦補償器とを備えたことを特徴とするモータ制御装置。 A motor control device for controlling a motor having a speed reducer,
A rotational speed sensor for detecting the rotational speed of the output shaft of the speed reducer;
A torque sensor for detecting the torque of the output shaft of the speed reducer;
Based on a rotational speed signal representing the rotational speed detected by the rotational speed sensor and a predetermined dynamic friction model of the speed reducer, a dynamic friction compensation signal for compensating the dynamic friction torque of the speed reducer is generated, and A dynamic friction compensator to be supplied to the motor;
Based on a torque signal representing the torque detected by the torque sensor and a motor inertia torque signal generated based on an angular acceleration signal representing an angular acceleration corresponding to the rotational speed of the output shaft of the speed reducer, A motor control device comprising: a static friction compensator that generates a static friction compensation signal that compensates for the static friction torque of the speed reducer and supplies the static friction compensation signal to the motor. 前記動摩擦モデルは、前記モータへの印加電圧を前記減速機の出力軸の回転速度についての関数として与えられる請求項１に記載のモータ制御装置。   The motor control device according to claim 1, wherein the dynamic friction model is provided with a voltage applied to the motor as a function of a rotational speed of an output shaft of the speed reducer. 前記静止摩擦補償信号は、前記モータ慣性トルク信号と、前記トルク信号に基づいて前記トルクを推定するトルク推定信号と、前記モータへの印加電圧を表す印加電圧信号とに基づいて生成される請求項１に記載のモータ制御装置。   The static friction compensation signal is generated based on the motor inertia torque signal, a torque estimation signal for estimating the torque based on the torque signal, and an applied voltage signal representing an applied voltage to the motor. The motor control device according to 1. 前記減速機は、歯車減速機である請求項１に記載のモータ制御装置。   The motor control device according to claim 1, wherein the speed reducer is a gear speed reducer. 前記歯車減速機は、波動歯車減速機である請求項４に記載のモータ制御装置。   The motor control device according to claim 4, wherein the gear reducer is a wave gear reducer. 減速機を有するモータを制御するモータ制御方法であって、
前記減速機の出力軸の回転速度を検出する回転速度検出工程と、
前記減速機の出力軸のトルクを検出するトルク検出工程と、
前記回転速度検出工程により検出された前記回転速度を表す回転速度信号と、予め定められた前記減速機の動摩擦モデルとに基づいて、前記減速機の動摩擦トルクを補償する動摩擦補償信号を生成して前記モータに供給する動摩擦補償工程と、
前記トルク検出工程により検出された前記トルクを表すトルク信号と、前記減速機の出力軸の回転速度に対応する角加速度を表す角加速度信号に基づいて生成されるモータ慣性トルク信号とに基づいて、前記減速機の静止摩擦トルクを補償する静止摩擦補償信号を生成して前記モータに供給する静止摩擦補償工程とを包含することを特徴とするモータ制御方法。 A motor control method for controlling a motor having a reduction gear,
A rotational speed detection step of detecting the rotational speed of the output shaft of the speed reducer;
A torque detection step of detecting torque of the output shaft of the speed reducer;
Based on a rotational speed signal representing the rotational speed detected by the rotational speed detecting step and a predetermined dynamic friction model of the speed reducer, a dynamic friction compensation signal for compensating the dynamic friction torque of the speed reducer is generated. A dynamic friction compensation step for supplying the motor;
Based on the torque signal representing the torque detected by the torque detection step and the motor inertia torque signal generated based on the angular acceleration signal representing the angular acceleration corresponding to the rotational speed of the output shaft of the speed reducer, And a static friction compensation step of generating a static friction compensation signal for compensating the static friction torque of the speed reducer and supplying the static friction compensation signal to the motor.