WO2021006038A1 - Torque compensation device and torque compensation method - Google Patents

Torque compensation device and torque compensation method Download PDF

Info

Publication number
WO2021006038A1
WO2021006038A1 PCT/JP2020/024788 JP2020024788W WO2021006038A1 WO 2021006038 A1 WO2021006038 A1 WO 2021006038A1 JP 2020024788 W JP2020024788 W JP 2020024788W WO 2021006038 A1 WO2021006038 A1 WO 2021006038A1
Authority
WO
WIPO (PCT)
Prior art keywords
torque
compensation
function
unit
joint angle
Prior art date
Application number
PCT/JP2020/024788
Other languages
French (fr)
Japanese (ja)
Inventor
友哉 中澤
貴弘 茂木
鉄也 田原
Original Assignee
アズビル株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by アズビル株式会社 filed Critical アズビル株式会社
Publication of WO2021006038A1 publication Critical patent/WO2021006038A1/en

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements

Definitions

  • the present invention relates to a torque compensating device and a torque compensating method for compensating for an error due to torque disturbance to a robot arm to which a torque sensor is attached.
  • a torque sensor is attached to each joint, and a robot arm that controls the torque detected by the torque sensor or the force estimated from the torque is known (see, for example, Patent Document 1).
  • the torque detected by the torque sensor may include an error due to the mechanical change of the speed reducer becoming a disturbance.
  • the speed reducer has a rotating mechanism such as a gear, and is an error that can occur even in a speed reducer having a wave gear, which is often used in a robot arm, for example.
  • Torque disturbances can change significantly even if the joint angle of the robot arm changes slightly, degrading the performance of torque control and force control. It is possible to design a mechanical mechanism that suppresses this torque disturbance, but the mechanism becomes complicated and the cost increases.
  • the present invention has been made to solve the above problems, and an object of the present invention is to provide a torque compensating device capable of compensating for an error due to torque disturbance regardless of a mechanical mechanism.
  • the torque compensating device includes a data acquisition unit for acquiring torque detected by a torque sensor attached to a robot arm and a joint angle of the robot arm when the torque sensor detects torque, and a rotation mechanism.
  • Torque compensation value for compensating for the error due to torque disturbance at the joint angle based on the compensation function which is a periodic function for the joint angle of the torque disturbance caused by the speed reducer and the joint angle acquired by the data acquisition unit.
  • Residual calculation unit that calculates the residual of the torque acquired by the data acquisition unit and the torque compensation value calculated by the compensation value calculation unit as the torque after compensation, and the residual calculation. It is characterized by including an output unit that outputs data indicating the compensated torque calculated by the unit.
  • FIG. It is a figure which shows the structural example of the torque compensating apparatus which concerns on Embodiment 1.
  • FIG. It is a flowchart which shows the estimation operation example by the torque compensator which concerns on Embodiment 1.
  • FIG. It is a figure which shows the operation example of the torque compensating apparatus which concerns on Embodiment 1, and is the figure which shows an example of a torque set and a joint angle set.
  • It is a flowchart which shows the compensation operation example by the torque compensation apparatus which concerns on Embodiment 1.
  • FIG. 1 is a diagram showing a configuration example of a torque compensating device according to the first embodiment.
  • the torque compensation device compensates for errors due to torque disturbance for a robot arm in which a torque sensor is attached to each joint.
  • the robot arm uses a speed reducer having a rotation mechanism such as a gear.
  • the torque disturbance is a torque disturbance caused by a speed reducer having a rotating mechanism.
  • the torque compensator includes a data acquisition unit 1, a preprocessing unit 2, an estimation unit 3, and a compensation unit 4.
  • the torque compensation device is realized by a processing circuit such as a system LSI (Large Scale Integration), a CPU (Central Processing Unit) that executes a program stored in a memory or the like, or the like.
  • a processing circuit such as a system LSI (Large Scale Integration), a CPU (Central Processing Unit) that executes a program stored in a memory or the like, or the like.
  • the compensation function used for the compensation is estimated in advance before the error due to the torque disturbance is compensated.
  • the data acquisition unit 1 acquires the torque detected by the torque sensor and the joint angle of the robot arm when the torque sensor detects the torque.
  • the pre-processing unit 2 performs pre-processing on the data indicating the acquisition result by the data acquisition unit 1.
  • FIG. 1 shows a case where the torque compensator is provided with the pretreatment unit 2.
  • the present invention is not limited to this, and the torque compensator may not be provided with the pretreatment unit 2.
  • the estimation unit 3 and the compensation unit 4 perform various processes using the data acquired by the data acquisition unit 1.
  • the estimation unit 3 estimates the compensation function based on the data after the preprocessing by the preprocessing unit 2.
  • the compensation function is a periodic function for the joint angle of torque disturbance caused by a speed reducer having a rotation mechanism. As shown in FIG. 1, the estimation unit 3 has a function estimation unit 31 and an output unit 32.
  • the function estimation unit 31 estimates the compensation function based on the data after the preprocessing by the preprocessing unit 2.
  • the output unit 32 outputs data indicating the compensation function estimated by the function estimation unit 31 to the outside.
  • the compensation unit 4 outputs data indicating the torque (torque after compensation) that compensates for the error due to torque disturbance based on the data after the preprocessing by the preprocessing unit 2 and the compensation function estimated by the estimation unit 3. As shown in FIG. 1, the compensation unit 4 includes a function reading unit 41, a compensation value calculation unit 42, a residual calculation unit 43, and an output unit 44.
  • the function reading unit 41 reads the compensation function estimated by the function estimation unit 31 from the data output by the output unit 32.
  • the compensation value calculation unit 42 calculates the torque compensation value at the joint angle based on the joint angle indicated by the compensation function read by the function reading unit 41 and the data after the preprocessing by the preprocessing unit 2.
  • the torque compensation value is a compensation value for compensating for an error due to torque disturbance.
  • the residual calculation unit 43 calculates the residual between the torque indicated by the data after the preprocessing by the preprocessing unit 2 and the torque compensation value calculated by the compensation value calculation unit 42 as the post-compensation torque.
  • the output unit 44 outputs data indicating the compensated torque calculated by the residual calculation unit 43 to the outside.
  • the data output by the output unit 44 is used for torque control, force control, and the like.
  • the estimation unit 3 is provided with the output unit 32, and the compensation unit 4 is provided with the function reading unit 41.
  • the output unit 32 and the function reading unit 41 are not indispensable, and the output unit 32 and the function reading unit are not essential.
  • the unit 41 may not be provided in the torque compensation device.
  • both the estimation unit 3 and the compensation unit 4 are provided in the torque compensation device.
  • the present invention is not limited to this, and the estimation unit 3 may not be provided in the torque compensation device, and the torque compensation device may compensate for an error due to torque disturbance by using a compensation function indicated by data input from the outside.
  • the torque compensating device since the torque disturbance is caused by the speed reducer having the rotation mechanism, it is considered that the torque disturbance has periodicity according to the rotation cycle. Therefore, in the torque compensating device according to the first embodiment, utilizing the fact that the torque disturbance has periodicity, the torque disturbance is estimated by the periodic function and the error due to the torque disturbance is compensated.
  • the torque disturbance changes as the joint angle of the robot arm changes. From this, it is considered that the torque disturbance is a function of the joint angle. Furthermore, considering that the torque disturbance has periodicity, it can be seen that the compensation function can be expressed by a periodic function. That is, by estimating the periodic function with the joint angle as the input and the torque compensation value as the output, the error due to the torque disturbance can be compensated. As a result, in the torque compensating device according to the first embodiment, it is possible to secure the performance of torque control and force control in the robot arm without adding a mechanical mechanism for suppressing torque disturbance.
  • the data acquisition unit 1 first detects the torque detected by the torque sensor and the torque sensor detects the torque.
  • the joint angle of the robot arm at the time of this is acquired (step ST201).
  • the purpose of the torque compensator is to compensate for an error due to torque disturbance caused by a speed reducer having a rotating mechanism. Therefore, it is desirable that the torque used in the estimation process by the torque compensator does not include disturbances other than torque disturbances caused by the reducer.
  • the torque compensator shall use torque that does not include the above disturbance. For example, when the torque detected by the torque sensor includes the torque due to gravity, it is necessary to remove the component in advance. Since the calculation of the gravitational torque component is a known technique as in Patent Document 2, for example, it can be removed by using those techniques.
  • the torque compensator can obtain a torque that does not substantially include the above-mentioned disturbance.
  • the preprocessing unit 2 performs preprocessing on the data indicating the acquisition result by the data acquisition unit 1 (step ST202).
  • the pretreatment by the pretreatment unit 2 may be any processing suitable for the situation, such as outlier removal or moving average.
  • the function estimation unit 31 estimates the compensation function based on the data after the preprocessing by the preprocessing unit 2 (step ST203). In the first embodiment, the function estimation unit 31 estimates the period of the torque disturbance, and then estimates the function (compensation function) for compensating for the error due to the torque disturbance for one cycle.
  • the compensation function is a function that minimizes the periodic function repeated for the number of cycles and the error due to torque disturbance.
  • Various forms can be considered as this compensation function.
  • the compensation function a step function, a piecewise linear function, a polynomial, a trigonometric function, or the like can be used.
  • the function estimation unit 31 estimates them by maximum likelihood estimation, least squares method, ensemble regression, Gaussian process regression, support vector regression, or the like.
  • the function estimation unit 31 estimates the period of the component included in the torque based on the data preprocessed by the preprocessing unit 2. There are several conceivable methods for estimating the period by the function estimation unit 31, and it can be estimated from, for example, the specifications of the speed reducer or the result of expressing the torque in the frequency domain by the discrete Fourier transform. After estimating the period, the function estimation unit 31 extracts the torque and the joint angle in the section for one cycle and uses it for estimating the compensation function.
  • the number of cycles of the component having a cycle in the torque is n
  • the section torque in which the torque (torque set) is divided for each cycle is t 1 , t 2 , ..., T n
  • x 1 , x 2 , ..., X n be the section joint angles that divide the joint angles (joint angle sets) for each cycle
  • ⁇ be the width of one section joint angle.
  • the section torque (t j ) in an arbitrary section (j ⁇ ⁇ 1, 2, ..., N ⁇ ) includes a plurality of data points (t j1 , t j2 , ...), And is an arbitrary section (t j1 , t j2 , ).
  • the joint angle interval (x j ) in j ⁇ ⁇ 1, 2, ..., N ⁇ ) includes a plurality of data points (x j1 , x j2 , ).
  • a compensation function that minimizes the error with the section torque in an arbitrary section is estimated, and the result is applied to other sections to perform torque compensation.
  • j 1.
  • the joint angle (x jk ) of the arbitrary data point (k) at j satisfies (j-1) ⁇ ⁇ ⁇ x jk ⁇ j ⁇ ⁇ . Even if the actually acquired joint angle value does not satisfy this condition, it may be translated so as to meet the condition.
  • any compensation function that minimizes equation (2) can be used. It is considered that it can be used for compensation even in the section. That is, the parameters of the compensation function that minimizes the error function may be obtained by the least squares method or the like.
  • FIG. 4 shows an example of the compensation function estimated by the function estimation unit 31.
  • the waveform shown by reference numeral 401 shows the waveform of the torque indicated by the data after the preprocessing by the preprocessing unit 2
  • the waveform shown by reference numeral 402 shows the waveform of the compensation function estimated by the function estimation unit 31. ..
  • the output unit 32 outputs data indicating the compensation function estimated by the function estimation unit 31 to the outside (step ST204).
  • the data acquisition unit 1 first detects the torque detected by the torque sensor and the torque sensor detects the torque.
  • the joint angle of the robot arm at the time of this is acquired (step ST501).
  • the preprocessing unit 2 performs preprocessing on the data indicating the acquisition result by the data acquisition unit 1 (step ST502).
  • the function reading unit 41 reads the compensation function estimated by the function estimation unit 31 from the data output by the output unit 32 (step ST503).
  • the compensation function it is possible to repeatedly use the one that has been read once for the same robot arm, and it may not be necessary to read it each time.
  • the compensation value calculation unit 42 calculates the torque compensation value at the joint angle based on the compensation function read by the function reading unit 41 and the joint angle indicated by the data preprocessed by the preprocessing unit 2. (Step ST504).
  • the torque compensation value when an arbitrary joint angle (x (hat)) is obtained is defined as y (hat).
  • the compensation value calculation unit 42 determines which section joint angle (x 1 ) the joint angle (x (hat)) corresponds to. That is, the compensation value calculation unit 42 divides the joint angle (x (hat)) by the width ( ⁇ ) of one section joint angle and obtains the remainder ( ⁇ ). Then, as in the following equation (4), the compensation value calculation unit 42 calculates the torque compensation value by substituting the remainder into the compensation function.
  • the residual calculation unit 43 calculates the residual between the torque indicated by the data preprocessed by the preprocessing unit 2 and the torque compensation value calculated by the compensation value calculation unit 42 as the post-compensation torque ( Step ST505).
  • the output unit 44 outputs data indicating the compensated torque calculated by the residual calculation unit 43 to the outside (step ST506).
  • the torque disturbance has periodicity, and by repeatedly using the periodic function estimated in one section of one cycle unit, the other sections can also be used. Compensation for the entire torque set including it becomes possible.
  • the estimation unit 3 estimates the compensation function using one section.
  • the estimation unit 3 may estimate the compensation function using a plurality of sections.
  • the estimation unit 3 obtains an error function in the target section and estimates a compensation function that minimizes the sum.
  • the estimation unit 3 can make an estimation that compensates the section important for control more intensively by applying an arbitrary weight to the error function of each section. As a result, improvement in control performance can be expected.
  • the torque compensating device can cope with fluctuations in torque disturbance due to operating conditions.
  • the torque compensator determines the torque detected by the torque sensor attached to the robot arm and the joint angle of the robot arm when the torque sensor detects the torque. Torque at the joint angle based on the data acquisition unit 1 to be acquired, the compensation function which is a periodic function for the joint angle of torque disturbance caused by the speed reducer having a rotation mechanism, and the joint angle acquired by the data acquisition unit 1. Compensates for the residual between the compensation value calculation unit 42 that calculates the torque compensation value for compensating for the error due to the disturbance, the torque acquired by the data acquisition unit 1, and the torque compensation value calculated by the compensation value calculation unit 42.
  • a residual calculation unit 43 calculated as a rear torque and an output unit 44 that outputs data indicating the compensated torque calculated by the residual calculation unit 43 are provided.
  • the torque compensating device according to the first embodiment can compensate for an error due to torque disturbance regardless of the mechanical mechanism.
  • the function applicable as the compensation function is not limited.
  • the function used as the compensation function is limited to the sine function.
  • the sine function itself is a periodic function, and an arbitrary periodic function can be expressed by superimposing the sine function, so that it is suitable as a compensation function.
  • the sine function is a periodic function, it is possible to estimate the function without dividing it into each period as in the first embodiment, and in the second embodiment, the function estimation is performed collectively for a plurality of sections. Further, it is preferable that the estimation unit 3 calculates the initial value before the function estimation. This is to streamline the estimation of the parameters representing the sine function.
  • FIG. 6 is a diagram showing a configuration example of the torque compensating device according to the second embodiment.
  • the torque compensating device according to the second embodiment shown in FIG. 6 has an initial value calculation unit 33 added to the torque compensating device according to the first embodiment shown in FIG.
  • Other configurations are the same, and only different parts will be described with the same reference numerals.
  • the initial value calculation unit 33 calculates the initial value of the parameter representing the sine function based on the data after the preprocessing by the preprocessing unit 2. As shown in FIG. 6, the initial value calculation unit 33 includes an initial amplitude calculation unit 331, an initial frequency calculation unit 332, and an initial phase calculation unit 333.
  • the initial amplitude calculation unit 331 calculates the initial value of the amplitude, which is a parameter representing the sine function, based on the data after the preprocessing by the preprocessing unit 2.
  • the initial frequency calculation unit 332 calculates the initial value of the frequency, which is a parameter representing the sine function, based on the data after the preprocessing by the preprocessing unit 2.
  • FIG. 6 shows a case where the initial frequency calculation unit 332 is provided after the initial amplitude calculation unit 331.
  • the present invention is not limited to this, and the initial frequency calculation unit 332 may be provided in front of the initial amplitude calculation unit 331.
  • the initial phase calculation unit 333 calculates the initial value of the phase, which is a parameter representing a sine function, based on the initial value of the amplitude calculated by the initial amplitude calculation unit 331 and the initial value of the frequency calculated by the initial frequency calculation unit 332. calculate.
  • the function estimation unit 31 estimates the compensation function based on the data after the preprocessing by the preprocessing unit 2 and the initial value calculated by the initial value calculation unit 33.
  • the compensation function is represented by the superposition of sine functions.
  • the initial value calculation unit 33 is a means for improving the efficiency of function estimation by the function estimation unit 31, and is not an essential configuration for the torque compensation device. Further, when the torque compensation device is not provided with the initial value calculation unit 33, the initial value of the parameter representing the sine function is an arbitrary value such as a random number.
  • the data acquisition unit 1 detects the torque detected by the torque sensor and the torque sensor detects the torque.
  • the joint angle of the robot arm at the time of this is acquired (step ST701).
  • the preprocessing unit 2 performs preprocessing on the data indicating the acquisition result by the data acquisition unit 1 (step ST702).
  • the processing in steps ST701 and 702 is the same as the processing in steps ST201 and 202 in the first embodiment.
  • the initial value calculation unit 33 calculates the initial value, which is a parameter representing the sine function, based on the data after the preprocessing by the preprocessing unit 2 (step ST703).
  • the initial value calculation unit 33 calculates the initial value, which is a parameter representing the sine function, based on the data after the preprocessing by the preprocessing unit 2 (step ST703).
  • the initial amplitude calculation unit 331 is a parameter representing a sine function based on the data after the preprocessing by the preprocessing unit 2.
  • the initial value of the amplitude is calculated (step ST801).
  • the initial amplitude calculation unit 331 first performs a discrete Fourier transform on the torque indicated by the data after the preprocessing by the preprocessing unit 2.
  • a discrete Fourier transform it is assumed that a fast Fourier transform (FFT: Fast Fourier Transform) that performs the discrete Fourier transform at high speed is used.
  • FFT Fast Fourier Transform
  • the torque waveform can be represented in the frequency domain from the joint angle domain.
  • the periodic function can be expressed by superimposing a simple sine function or cosine function, and considering that the torque disturbance has periodicity, it is possible to estimate the function from the result of expressing the torque in the frequency domain. Understand.
  • the sine function among the trigonometric functions is used.
  • the initial amplitude calculation unit 331 obtains the initial value (a init ) of the amplitude by taking the absolute value of the peak based on the torque expressed in the frequency domain. Since it can be said that the magnitude of the absolute value of the peak in the frequency domain is the magnitude of the influence on the function, it is efficient to apply the compensation from the frequency having the largest absolute value. That is, the initial amplitude calculation unit 331 selects the absolute value of the largest peak as the initial value of the amplitude.
  • the initial frequency calculation unit 332 calculates the initial value of the frequency, which is a parameter representing the sine function, based on the data after the preprocessing by the preprocessing unit 2 (step ST802). At this time, the initial frequency calculation unit 332 first performs a discrete Fourier transform on the torque indicated by the data after the preprocessing by the preprocessing unit 2. Here, it is assumed that the fast Fourier transform (FFT) is used as the discrete Fourier transform. Then, the initial frequency calculation unit 332 sets the frequency having the absolute value of the peak as the initial value ( init ) of the frequency based on the torque expressed in the frequency domain.
  • FFT fast Fourier transform
  • the initial phase calculation unit 333 is the initial phase, which is a parameter representing a sine function, based on the initial value of the amplitude calculated by the initial amplitude calculation unit 331 and the initial value of the frequency calculated by the initial frequency calculation unit 332. Calculate the value (step ST803).
  • the initial phase calculation unit 333 uses the initial value of the amplitude calculated by the initial amplitude calculation unit 331 and the initial value of the frequency calculated by the initial frequency calculation unit 332, and uses the initial periodic function (l (x). )) Is generated, and the initial value ( pinit ) of the phase is obtained by using the initial periodic function.
  • the initial phase calculation unit 333 generates an initial periodic function using the frequency that is considered to have a large influence on the frequency component of the torque and its amplitude. Therefore, the initial phase calculation unit 333 may obtain the phase difference between the initial periodic function and the torque, and use the phase difference as the initial value of the phase.
  • the initial phase calculation unit 333 can obtain the phase difference by the cross-correlation function of the initial periodic function and the torque. That is, the phase difference when the cross-correlation function is maximized is the phase difference when the initial periodic function and the torque waveform overlap most, and the initial phase calculation unit 333 sets this as the initial value of the phase.
  • the function estimation unit 31 then returns to the data after the preprocessing by the preprocessing unit 2 and the initial value calculated by the initial value calculation unit 33. , Estimate the compensation function (step ST704).
  • the function estimation unit 31 first uses the initial values (amplitude initial value, frequency initial value, and phase initial value) calculated by the initial value calculation unit 33, and is caused by a speed reducer having a rotation mechanism. Estimate appropriate parameters (amplitude (a), frequency (f) and phase (p)) as a sine function to compensate for errors due to torque disturbances.
  • an appropriate parameter as a sine function for compensating for the error due to the torque disturbance is to minimize the function (L (x)) showing the following equation (6).
  • the joint angle (x) and the torque (t) include N data points, and t j indicates the value of the torque with respect to the joint angle (x j ).
  • the function estimation unit 31 estimates the parameters that minimize the above function by the least squares method.
  • the function estimation unit 31 uses the initial values (initial value of amplitude, initial value of frequency, and initial value of phase) calculated by the initial value calculation unit 33 as the initial value at the time of the above estimation.
  • the function estimation unit 31 repeats the above process, estimates the parameters of the trigonometric function that compensates for each frequency component included in the torque, and obtains a plurality of trigonometric functions.
  • the function estimation unit 31 may simultaneously estimate those parameters by using a plurality of sine functions at the time of estimation, or may further repeat the estimation to increase the number of sine functions.
  • the function estimation unit 31 obtains a torque compensation value (y (hat)) for compensating for an error due to torque disturbance at an arbitrary joint angle (x (hat)) from the following equation (7).
  • m represents a number of sinusoidal function estimator 31 is used to estimate
  • a i denotes the amplitude is a parameter representing the i-th sinusoidal
  • f i is the i-th sinusoidal
  • p i denotes the phase is a parameter representing the i-th sine function.
  • the output unit 32 outputs data indicating the compensation function estimated by the function estimation unit 31 (step ST705).
  • the frequency component can be compensated by performing approximation by a sine function on the premise that the torque disturbance is a periodic function of the joint angle.
  • the sine function used for compensation is generated by parameters that minimize the error with torque disturbance, and the torque compensation device efficiently estimates the compensation function by calculating the initial value in advance. It is possible.
  • the robot arm is a robot arm having a rotary joint
  • the robot arm is not limited to this, and may be any robot arm having a rotation drive mechanism to which a torque sensor is attached.
  • a robot arm having a multi-axis configuration such as a scalar type robot arm, or a parallel link.
  • a type robot arm or a robot arm having only one axis may be used.
  • the torque compensating device can compensate for an error due to torque disturbance regardless of a mechanical mechanism, and is used as a torque compensating device for compensating for an error due to torque disturbance to a robot arm to which a torque sensor is attached. Suitable for.

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Manipulator (AREA)

Abstract

The present invention includes: a data acquisition unit (1) for acquiring a torque detected by a torque sensor mounted on a robot arm and a joint angle of the robot arm when the torque sensor detected the torque; a compensation value calculation unit (42) for calculating, on the basis of a compensation function that is a periodic function of a torque disturbance, caused by a speed reducer having a rotating mechanism, with respect to the joint angle, and the joint angle acquired by the data acquisition unit (1), a torque compensation value for compensating an error in the joint angle due to the torque disturbance; a residual error calculation unit (43) for calculating, as a compensated torque, a residual error between the torque acquired by the data acquisition unit (1) and the torque compensation value calculated by the compensation value calculation unit (42); and an output unit (44) for outputting data showing the compensated torque calculated by the residual error calculation unit (43).

Description

トルク補償装置及びトルク補償方法Torque compensation device and torque compensation method
　この発明は、トルクセンサが取付けられたロボットアームに対してトルク外乱による誤差の補償を行うトルク補償装置及びトルク補償方法に関する。 The present invention relates to a torque compensating device and a torque compensating method for compensating for an error due to torque disturbance to a robot arm to which a torque sensor is attached.
　各関節にトルクセンサが取付けられ、このトルクセンサにより検出されたトルク、又は当該トルクから推定した力を制御するロボットアームが知られている（例えば特許文献１参照）。 A torque sensor is attached to each joint, and a robot arm that controls the torque detected by the torque sensor or the force estimated from the torque is known (see, for example, Patent Document 1).
特開２００６－０００９５５号公報Japanese Unexamined Patent Publication No. 2006-000955
　しかしながら、ロボットアームが減速機を用いている場合、トルクセンサにより検出されたトルクは、減速機の機械的変化が外乱となって誤差を含むことがある。これは、減速機が歯車のような回転機構を持つことに起因しており、例えばロボットアームで使われることが多い波動歯車を持つ減速機でも起こり得る誤差である。トルク外乱は、ロボットアームの関節角が僅かに変わっても大きく変わることがあり、トルク制御及び力制御の性能を劣化させる。このトルク外乱を抑制する機械的な機構を設計することは可能であるが、機構が複雑になり、また、コストも上昇する。 However, when the robot arm uses a speed reducer, the torque detected by the torque sensor may include an error due to the mechanical change of the speed reducer becoming a disturbance. This is due to the fact that the speed reducer has a rotating mechanism such as a gear, and is an error that can occur even in a speed reducer having a wave gear, which is often used in a robot arm, for example. Torque disturbances can change significantly even if the joint angle of the robot arm changes slightly, degrading the performance of torque control and force control. It is possible to design a mechanical mechanism that suppresses this torque disturbance, but the mechanism becomes complicated and the cost increases.
　この発明は、上記のような課題を解決するためになされたもので、機械的な機構に依らず、トルク外乱による誤差を補償可能なトルク補償装置を提供することを目的としている。 The present invention has been made to solve the above problems, and an object of the present invention is to provide a torque compensating device capable of compensating for an error due to torque disturbance regardless of a mechanical mechanism.
　この発明に係るトルク補償装置は、ロボットアームに取付けられたトルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際の当該ロボットアームの関節角を取得するデータ取得部と、回転機構を持つ減速機に起因するトルク外乱の関節角に対する周期関数である補償関数、及びデータ取得部により取得された関節角に基づいて、当該関節角におけるトルク外乱による誤差を補償するためのトルク補償値を算出する補償値算出部と、データ取得部により取得されたトルクと、補償値算出部により算出されたトルク補償値との残差を補償後トルクとして算出する残差算出部と、残差算出部により算出された補償後トルクを示すデータを出力する出力部とを備えたことを特徴とする。 The torque compensating device according to the present invention includes a data acquisition unit for acquiring torque detected by a torque sensor attached to a robot arm and a joint angle of the robot arm when the torque sensor detects torque, and a rotation mechanism. Torque compensation value for compensating for the error due to torque disturbance at the joint angle based on the compensation function which is a periodic function for the joint angle of the torque disturbance caused by the speed reducer and the joint angle acquired by the data acquisition unit. Residual calculation unit that calculates the residual of the torque acquired by the data acquisition unit and the torque compensation value calculated by the compensation value calculation unit as the torque after compensation, and the residual calculation. It is characterized by including an output unit that outputs data indicating the compensated torque calculated by the unit.
　この発明によれば、上記のように構成したので、機械的な機構に依らず、トルク外乱による誤差を補償可能となる。 According to the present invention, since it is configured as described above, it is possible to compensate for an error due to torque disturbance regardless of the mechanical mechanism.
実施の形態１に係るトルク補償装置の構成例を示す図である。It is a figure which shows the structural example of the torque compensating apparatus which concerns on Embodiment 1. FIG. 実施の形態１に係るトルク補償装置による推定動作例を示すフローチャートである。It is a flowchart which shows the estimation operation example by the torque compensator which concerns on Embodiment 1. FIG. 実施の形態１に係るトルク補償装置の動作例を示す図であり、トルク集合と関節角集合の一例を示す図である。It is a figure which shows the operation example of the torque compensating apparatus which concerns on Embodiment 1, and is the figure which shows an example of a torque set and a joint angle set. 実施の形態１における関数推定部による動作例を示す図である。It is a figure which shows the operation example by the function estimation part in Embodiment 1. 実施の形態１に係るトルク補償装置による補償動作例を示すフローチャートである。It is a flowchart which shows the compensation operation example by the torque compensation apparatus which concerns on Embodiment 1. 実施の形態２に係るトルク補償装置の構成例を示す図である。It is a figure which shows the structural example of the torque compensating apparatus which concerns on Embodiment 2. 実施の形態２に係るトルク補償装置による推定動作例を示すフローチャートである。It is a flowchart which shows the estimation operation example by the torque compensator which concerns on Embodiment 2. 実施の形態１に係るトルク補償装置による初期値算出動作例を示すフローチャートである。It is a flowchart which shows the initial value calculation operation example by the torque compensator which concerns on Embodiment 1. FIG.
　以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
　図１は実施の形態１に係るトルク補償装置の構成例を示す図である。
　トルク補償装置は、各関節にトルクセンサが取付けられたロボットアームに対してトルク外乱による誤差の補償を行う。ロボットアームは歯車のような回転機構を持つ減速機を用いている。また、トルク外乱は、回転機構を持つ減速機に起因するトルク外乱である。トルク補償装置は、図１に示すように、データ取得部１、前処理部２、推定部３及び補償部４を備えている。なお、トルク補償装置は、システムＬＳＩ（Ｌａｒｇｅ　Ｓｃａｌｅ　Ｉｎｔｅｇｒａｔｉｏｎ）等の処理回路、又はメモリ等に記憶されたプログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）等により実現される。図１に示すトルク補償装置では、トルク外乱による誤差の補償を行う前に、事前に、その補償に用いる補償関数の推定を行う。 Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
Embodiment 1.
FIG. 1 is a diagram showing a configuration example of a torque compensating device according to the first embodiment.
The torque compensation device compensates for errors due to torque disturbance for a robot arm in which a torque sensor is attached to each joint. The robot arm uses a speed reducer having a rotation mechanism such as a gear. Further, the torque disturbance is a torque disturbance caused by a speed reducer having a rotating mechanism. As shown in FIG. 1, the torque compensator includes a data acquisition unit 1, a preprocessing unit 2, an estimation unit 3, and a compensation unit 4. The torque compensation device is realized by a processing circuit such as a system LSI (Large Scale Integration), a CPU (Central Processing Unit) that executes a program stored in a memory or the like, or the like. In the torque compensation device shown in FIG. 1, the compensation function used for the compensation is estimated in advance before the error due to the torque disturbance is compensated.
　データ取得部１は、トルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際のロボットアームの関節角を取得する。 The data acquisition unit 1 acquires the torque detected by the torque sensor and the joint angle of the robot arm when the torque sensor detects the torque.
　前処理部２は、データ取得部１による取得結果を示すデータに対して前処理を行う。
　図１では、トルク補償装置に前処理部２が設けられた場合を示している。しかしながら、これに限らず、トルク補償装置に前処理部２が設けられていなくてもよい。この場合、推定部３及び補償部４は、データ取得部１により取得されたデータを用いて各種処理を行う。 The pre-processing unit 2 performs pre-processing on the data indicating the acquisition result by the data acquisition unit 1.
FIG. 1 shows a case where the torque compensator is provided with the pretreatment unit 2. However, the present invention is not limited to this, and the torque compensator may not be provided with the pretreatment unit 2. In this case, the estimation unit 3 and the compensation unit 4 perform various processes using the data acquired by the data acquisition unit 1.
　推定部３は、前処理部２による前処理後のデータに基づいて、補償関数を推定する。補償関数は、回転機構を持つ減速機に起因するトルク外乱の関節角に対する周期関数である。推定部３は、図１に示すように、関数推定部３１及び出力部３２を有している。 The estimation unit 3 estimates the compensation function based on the data after the preprocessing by the preprocessing unit 2. The compensation function is a periodic function for the joint angle of torque disturbance caused by a speed reducer having a rotation mechanism. As shown in FIG. 1, the estimation unit 3 has a function estimation unit 31 and an output unit 32.
　関数推定部３１は、前処理部２による前処理後のデータに基づいて、補償関数を推定する。
　出力部３２は、関数推定部３１により推定された補償関数を示すデータを外部に出力する。 The function estimation unit 31 estimates the compensation function based on the data after the preprocessing by the preprocessing unit 2.
The output unit 32 outputs data indicating the compensation function estimated by the function estimation unit 31 to the outside.
　補償部４は、前処理部２による前処理後のデータ及び推定部３により推定された補償関数に基づいて、トルク外乱による誤差を補償したトルク（補償後トルク）を示すデータを出力する。補償部４は、図１に示すように、関数読込部４１、補償値算出部４２、残差算出部４３及び出力部４４を有している。 The compensation unit 4 outputs data indicating the torque (torque after compensation) that compensates for the error due to torque disturbance based on the data after the preprocessing by the preprocessing unit 2 and the compensation function estimated by the estimation unit 3. As shown in FIG. 1, the compensation unit 4 includes a function reading unit 41, a compensation value calculation unit 42, a residual calculation unit 43, and an output unit 44.
　関数読込部４１は、出力部３２により出力されたデータから、関数推定部３１により推定された補償関数を読込む。 The function reading unit 41 reads the compensation function estimated by the function estimation unit 31 from the data output by the output unit 32.
　補償値算出部４２は、関数読込部４１により読込まれた補償関数及び前処理部２による前処理後のデータが示す関節角に基づいて、当該関節角におけるトルク補償値を算出する。トルク補償値は、トルク外乱による誤差を補償するための補償値である。 The compensation value calculation unit 42 calculates the torque compensation value at the joint angle based on the joint angle indicated by the compensation function read by the function reading unit 41 and the data after the preprocessing by the preprocessing unit 2. The torque compensation value is a compensation value for compensating for an error due to torque disturbance.
　残差算出部４３は、前処理部２による前処理後のデータが示すトルクと、補償値算出部４２により算出されたトルク補償値との残差を補償後トルクとして算出する。 The residual calculation unit 43 calculates the residual between the torque indicated by the data after the preprocessing by the preprocessing unit 2 and the torque compensation value calculated by the compensation value calculation unit 42 as the post-compensation torque.
　出力部４４は、残差算出部４３により算出された補償後トルクを示すデータを外部に出力する。出力部４４により出力されたデータは、トルク制御又は力制御等に利用される。 The output unit 44 outputs data indicating the compensated torque calculated by the residual calculation unit 43 to the outside. The data output by the output unit 44 is used for torque control, force control, and the like.
　なお図１では、推定部３に出力部３２が設けられ、補償部４に関数読込部４１が設けられている。しかしながら、これに限らず、推定部３による推定結果を示すデータを内部で保持してそのまま補償部４で用いる場合には出力部３２及び関数読込部４１は必須ではなく、出力部３２及び関数読込部４１はトルク補償装置に設けられていなくてもよい。 Note that in FIG. 1, the estimation unit 3 is provided with the output unit 32, and the compensation unit 4 is provided with the function reading unit 41. However, not limited to this, when the data showing the estimation result by the estimation unit 3 is internally held and used as it is in the compensation unit 4, the output unit 32 and the function reading unit 41 are not indispensable, and the output unit 32 and the function reading unit are not essential. The unit 41 may not be provided in the torque compensation device.
　また図１では、トルク補償装置に推定部３及び補償部４の両方が設けられている。しかしながら、これに限らず、推定部３についてはトルク補償装置に設けず、トルク補償装置は外部から入力されたデータが示す補償関数を用いてトルク外乱による誤差の補償を行ってもよい。 Further, in FIG. 1, both the estimation unit 3 and the compensation unit 4 are provided in the torque compensation device. However, the present invention is not limited to this, and the estimation unit 3 may not be provided in the torque compensation device, and the torque compensation device may compensate for an error due to torque disturbance by using a compensation function indicated by data input from the outside.
　次に、図１に示す実施の形態１に係るトルク補償装置の動作例について説明する。
　ここで、トルク外乱は、回転機構を持つ減速機に起因することから、その回転周期に応じた周期性を持つと考えられる。そこで、実施の形態１に係るトルク補償装置では、トルク外乱が周期性を持つことを利用し、周期関数によってトルク外乱を推定してトルク外乱による誤差の補償を行う。 Next, an operation example of the torque compensating device according to the first embodiment shown in FIG. 1 will be described.
Here, since the torque disturbance is caused by the speed reducer having the rotation mechanism, it is considered that the torque disturbance has periodicity according to the rotation cycle. Therefore, in the torque compensating device according to the first embodiment, utilizing the fact that the torque disturbance has periodicity, the torque disturbance is estimated by the periodic function and the error due to the torque disturbance is compensated.
　また上述の通り、ロボットアームの関節角の変化に伴ってトルク外乱は変化する。このことから、トルク外乱は、関節角の関数であると考えられる。更に、トルク外乱が周期性を持つことを考えると、補償関数は周期関数で表せられることがわかる。すなわち、関節角を入力としトルク補償値を出力とする周期関数を推定することで、トルク外乱による誤差を補償可能となる。これにより、実施の形態１に係るトルク補償装置では、トルク外乱を抑制する機械的な機構を追加せずに、ロボットアームにおけるトルク制御及び力制御の性能を確保可能となる。 Also, as mentioned above, the torque disturbance changes as the joint angle of the robot arm changes. From this, it is considered that the torque disturbance is a function of the joint angle. Furthermore, considering that the torque disturbance has periodicity, it can be seen that the compensation function can be expressed by a periodic function. That is, by estimating the periodic function with the joint angle as the input and the torque compensation value as the output, the error due to the torque disturbance can be compensated. As a result, in the torque compensating device according to the first embodiment, it is possible to secure the performance of torque control and force control in the robot arm without adding a mechanical mechanism for suppressing torque disturbance.
　まず、図１に示す実施の形態１に係るトルク補償装置による推定処理例について、図２を参照しながら説明する。
　図１に示す実施の形態１に係るトルク補償装置による推定処理例では、図２に示すように、まず、データ取得部１は、トルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際のロボットアームの関節角を取得する（ステップＳＴ２０１）。 First, an example of estimation processing by the torque compensator according to the first embodiment shown in FIG. 1 will be described with reference to FIG.
In the estimation processing example by the torque compensating device according to the first embodiment shown in FIG. 1, first, as shown in FIG. 2, the data acquisition unit 1 first detects the torque detected by the torque sensor and the torque sensor detects the torque. The joint angle of the robot arm at the time of this is acquired (step ST201).
　なお、トルク補償装置では、回転機構を持つ減速機に起因するトルク外乱による誤差を補償することを目的としている。よって、トルク補償装置による推定処理において用いられるトルクは、減速機に起因するトルク外乱以外の外乱が含まれていないことが望ましい。以下では、トルク補償装置は、上記外乱が含まれていないトルクを用いるものとする。例えば、トルクセンサにより検出されたトルクが重力によるトルクを含む場合は、事前にその成分を除去する必要がある。この重力トルク成分の算出は例えば特許文献２のように公知の技術なので、それらの技術を利用して除去が可能である。また、ロボットアームにかかる慣性及びコリオリ力の影響を十分に抑制するため、トルク値を取得する際はロボットアームの速度と加速度を十分小さく抑えるとよい。以上のようにすることで、トルク補償装置は上記外乱が事実上含まれていないトルクを得ることができる。
特開平０１－０６６７１５号公報 The purpose of the torque compensator is to compensate for an error due to torque disturbance caused by a speed reducer having a rotating mechanism. Therefore, it is desirable that the torque used in the estimation process by the torque compensator does not include disturbances other than torque disturbances caused by the reducer. In the following, the torque compensator shall use torque that does not include the above disturbance. For example, when the torque detected by the torque sensor includes the torque due to gravity, it is necessary to remove the component in advance. Since the calculation of the gravitational torque component is a known technique as in Patent Document 2, for example, it can be removed by using those techniques. Further, in order to sufficiently suppress the influence of the inertia and the Coriolis force applied to the robot arm, it is preferable to sufficiently suppress the speed and acceleration of the robot arm when acquiring the torque value. By doing so, the torque compensator can obtain a torque that does not substantially include the above-mentioned disturbance.
Japanese Unexamined Patent Publication No. 01-066715
　次いで、前処理部２は、データ取得部１による取得結果を示すデータに対して前処理を行う（ステップＳＴ２０２）。前処理部２による前処理は、外れ値除去又は移動平均等、その状況に適した処理であればよい。 Next, the preprocessing unit 2 performs preprocessing on the data indicating the acquisition result by the data acquisition unit 1 (step ST202). The pretreatment by the pretreatment unit 2 may be any processing suitable for the situation, such as outlier removal or moving average.
　次いで、関数推定部３１は、前処理部２による前処理後のデータに基づいて、補償関数を推定する（ステップＳＴ２０３）。実施の形態１では、関数推定部３１は、トルク外乱の持つ周期を推定した後、１周期分のトルク外乱による誤差を補償するための関数（補償関数）を推定する。 Next, the function estimation unit 31 estimates the compensation function based on the data after the preprocessing by the preprocessing unit 2 (step ST203). In the first embodiment, the function estimation unit 31 estimates the period of the torque disturbance, and then estimates the function (compensation function) for compensating for the error due to the torque disturbance for one cycle.
　補償関数は、周期数分繰返した周期関数とトルク外乱による誤差とを最小化するような関数である。この補償関数としては様々な形態が考えられる。例えば、補償関数として、階段関数、区分線形関数、多項式又は三角関数等を用いることができる。そして、関数推定部３１は、それらを最尤推定、最小二乗法、アンサンブル回帰、ガウス過程回帰又はサポートベクター回帰等で推定する。 The compensation function is a function that minimizes the periodic function repeated for the number of cycles and the error due to torque disturbance. Various forms can be considered as this compensation function. For example, as the compensation function, a step function, a piecewise linear function, a polynomial, a trigonometric function, or the like can be used. Then, the function estimation unit 31 estimates them by maximum likelihood estimation, least squares method, ensemble regression, Gaussian process regression, support vector regression, or the like.
　以下では、一例として、補償関数として３次元多項式を用いた場合を示す。補償関数をｆ（ｘ）とし、そのパラメータをａ，ｂ，ｃ，ｄとすると、３次多項式の補償関数は下式（１）で表される。
ｆ（ｘ）＝ａｘ３＋ｂｘ２＋ｃｘ＋ｄ　　　　　　（１） In the following, as an example, a case where a three-dimensional polynomial is used as a compensation function is shown. Assuming that the compensation function is f (x) and its parameters are a, b, c, and d, the compensation function of the cubic polynomial is expressed by the following equation (1).
f (x) = ax 3 + bx 2 + cx + d (1)
　関数推定部３１は、まず、前処理部２による前処理が行われたデータに基づいて、トルクに含まれる成分の周期の推定を行う。関数推定部３１による周期の推定方法としては幾つか考えられ、例えば減速機の仕様又はトルクを離散フーリエ変換によって周波数領域で表した結果等から推定可能である。関数推定部３１は、周期を推定した後、その１周期分の区間におけるトルク及び関節角を取出し、補償関数の推定に用いる。 First, the function estimation unit 31 estimates the period of the component included in the torque based on the data preprocessed by the preprocessing unit 2. There are several conceivable methods for estimating the period by the function estimation unit 31, and it can be estimated from, for example, the specifications of the speed reducer or the result of expressing the torque in the frequency domain by the discrete Fourier transform. After estimating the period, the function estimation unit 31 extracts the torque and the joint angle in the section for one cycle and uses it for estimating the compensation function.
　図３に示すように、トルクのうちの周期を持つ成分の周期数をｎとし、トルク（トルク集合）を周期毎に区切った区間トルクをｔ１，ｔ２，・・・，ｔｎとし、関節角（関節角集合）を当該周期毎に区切った区間関節角をｘ１，ｘ２，・・・，ｘｎとし、一つの区間関節角の幅をδとする。なお、任意の区間（ｊ∈｛１，２，・・・，ｎ｝）における区間トルク（ｔｊ）は複数のデータ点（ｔｊ１，ｔｊ２，・・・）を含み、任意の区間（ｊ∈｛１，２，・・・，ｎ｝）における関節角区間（ｘｊ）は複数のデータ点（ｘｊ１，ｘｊ２，・・・）を含む。
　ここでは、補償関数の推定例として、任意の区間における区間トルクとの誤差を最小化するような補償関数を推定し、その結果を他の区間でも適用してトルク補償を行うこととする。以下では、ｊ＝１として説明する。また、説明を平易にするため、ｊにおける任意のデータ点（ｋ）の関節角（ｘｊｋ）は、（ｊ－１）×δ≦ｘｊｋ＜ｊ×δを満たすものとする。なお、実際に取得した関節角の値がこの条件を満たしていなかったとしても、条件に合うように平行移動させればよい。 As shown in FIG. 3, the number of cycles of the component having a cycle in the torque is n, and the section torque in which the torque (torque set) is divided for each cycle is t 1 , t 2 , ..., T n . Let x 1 , x 2 , ..., X n be the section joint angles that divide the joint angles (joint angle sets) for each cycle, and let δ be the width of one section joint angle. The section torque (t j ) in an arbitrary section (j ∈ {1, 2, ..., N}) includes a plurality of data points (t j1 , t j2 , ...), And is an arbitrary section (t j1 , t j2 , ...). The joint angle interval (x j ) in j ∈ {1, 2, ..., N}) includes a plurality of data points (x j1 , x j2 , ...).
Here, as an estimation example of the compensation function, a compensation function that minimizes the error with the section torque in an arbitrary section is estimated, and the result is applied to other sections to perform torque compensation. Hereinafter, it will be described as j = 1. Further, for the sake of simplicity, it is assumed that the joint angle (x jk ) of the arbitrary data point (k) at j satisfies (j-1) × δ ≦ x jk <j × δ. Even if the actually acquired joint angle value does not satisfy this condition, it may be translated so as to meet the condition.
　まず、前提として、下式（２），（３）に示す誤差関数であるｌ（ｘ）を最小化するような補償関数を推定する。なお、｜ｖ｜はベクトルであるｖの大きさ（絶対値）を表す。
ｌ（ｘ１）＝｜ｔ１－ｆ（ｘ１）｜　　　　　　（２）
ｆ（ｘ１）＝｛ｆ（ｘ１１），ｆ（ｘ１２），・・・｝　　（３） First, as a premise, a compensation function that minimizes l (x), which is an error function shown in the following equations (2) and (3), is estimated. Note that | v | represents the magnitude (absolute value) of v, which is a vector.
l (x 1 ) = | t 1- f (x 1 ) | (2)
f (x 1 ) = {f (x 11 ), f (x 12 ), ...} (3)
　トルク集合は、トルクの周期性を利用してその１周期分毎に区切られた区間トルクの集合であることを踏まえると、式（２）を最小化するような補償関数であれば、他の区間においても補償に利用可能であると考えられる。すなわち、誤差関数を最小化するような補償関数のパラメータを最小二乗法等によって求めればよい。 Considering that the torque set is a set of interval torques divided for each cycle using the periodicity of torque, any compensation function that minimizes equation (2) can be used. It is considered that it can be used for compensation even in the section. That is, the parameters of the compensation function that minimizes the error function may be obtained by the least squares method or the like.
　以上の例では、補償関数を１区間分だけ推定したが、この補償関数を他の区間にもそのまま周期的に適用するので、周期関数となるような補償関数を推定したことと同じになる。
　図４に、関数推定部３１により推定される補償関数の一例を示す。図４において、符号４０１に示す波形は前処理部２による前処理後のデータが示すトルクの波形を示し、符号４０２に示す波形は関数推定部３１により推定された補償関数の波形を示している。 In the above example, the compensation function is estimated for only one section, but since this compensation function is periodically applied to other sections as it is, it is the same as estimating the compensation function that becomes a periodic function.
FIG. 4 shows an example of the compensation function estimated by the function estimation unit 31. In FIG. 4, the waveform shown by reference numeral 401 shows the waveform of the torque indicated by the data after the preprocessing by the preprocessing unit 2, and the waveform shown by reference numeral 402 shows the waveform of the compensation function estimated by the function estimation unit 31. ..
　次いで、出力部３２は、関数推定部３１により推定された補償関数を示すデータを外部に出力する（ステップＳＴ２０４）。 Next, the output unit 32 outputs data indicating the compensation function estimated by the function estimation unit 31 to the outside (step ST204).
　次に、図１に示す実施の形態１に係るトルク補償装置による補償処理例について、図５を参照しながら説明する。
　図１に示す実施の形態１に係るトルク補償装置による補償処理例では、図５に示すように、まず、データ取得部１は、トルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際のロボットアームの関節角を取得する（ステップＳＴ５０１）。 Next, an example of compensation processing by the torque compensation device according to the first embodiment shown in FIG. 1 will be described with reference to FIG.
In the example of compensation processing by the torque compensating device according to the first embodiment shown in FIG. 1, first, as shown in FIG. 5, the data acquisition unit 1 first detects the torque detected by the torque sensor and the torque sensor detects the torque. The joint angle of the robot arm at the time of this is acquired (step ST501).
　次いで、前処理部２は、データ取得部１による取得結果を示すデータに対して前処理を行う（ステップＳＴ５０２）。 Next, the preprocessing unit 2 performs preprocessing on the data indicating the acquisition result by the data acquisition unit 1 (step ST502).
　また、関数読込部４１は、出力部３２により出力されたデータから、関数推定部３１により推定された補償関数を読込む（ステップＳＴ５０３）。なお、補償関数は、同一のロボットアームに対しては一度読込んだものを繰返し用いることが可能であり、毎回読込む必要はないものとしてもよい。 Further, the function reading unit 41 reads the compensation function estimated by the function estimation unit 31 from the data output by the output unit 32 (step ST503). As the compensation function, it is possible to repeatedly use the one that has been read once for the same robot arm, and it may not be necessary to read it each time.
　次いで、補償値算出部４２は、関数読込部４１により読込まれた補償関数及び前処理部２により前処理が行われたデータが示す関節角に基づいて、当該関節角におけるトルク補償値を算出する（ステップＳＴ５０４）。 Next, the compensation value calculation unit 42 calculates the torque compensation value at the joint angle based on the compensation function read by the function reading unit 41 and the joint angle indicated by the data preprocessed by the preprocessing unit 2. (Step ST504).
　ここで、任意の関節角（ｘ（ハット））が得られた際のトルク補償値をｙ（ハット）とする。まず、補償値算出部４２は、関節角（ｘ（ハット））が、どの区間関節角（ｘ１）に相当するかを判定する。すなわち、補償値算出部４２は、一つの区間関節角の幅（δ）で関節角（ｘ（ハット））を割り、その余り（λ）を求める。そして、下式（４）のように、補償値算出部４２は、その余りを補償関数に代入することでトルク補償値を算出する。
ｙ（ハット）＝ｆ（λ）＝ａλ３＋ｂλ２＋ｃλ＋ｄ　　（４） Here, the torque compensation value when an arbitrary joint angle (x (hat)) is obtained is defined as y (hat). First, the compensation value calculation unit 42 determines which section joint angle (x 1 ) the joint angle (x (hat)) corresponds to. That is, the compensation value calculation unit 42 divides the joint angle (x (hat)) by the width (δ) of one section joint angle and obtains the remainder (λ). Then, as in the following equation (4), the compensation value calculation unit 42 calculates the torque compensation value by substituting the remainder into the compensation function.
y (hat) = f (λ) = aλ 3 + bλ 2 + cλ + d (4)
　次いで、残差算出部４３は、前処理部２により前処理が行われたデータが示すトルクと、補償値算出部４２により算出されたトルク補償値との残差を補償後トルクとして算出する（ステップＳＴ５０５）。 Next, the residual calculation unit 43 calculates the residual between the torque indicated by the data preprocessed by the preprocessing unit 2 and the torque compensation value calculated by the compensation value calculation unit 42 as the post-compensation torque ( Step ST505).
　次いで、出力部４４は、残差算出部４３により算出された補償後トルクを示すデータを外部に出力する（ステップＳＴ５０６）。 Next, the output unit 44 outputs data indicating the compensated torque calculated by the residual calculation unit 43 to the outside (step ST506).
　このように、実施の形態１に係るトルク補償装置では、トルク外乱が周期性を持つことを利用し、１周期単位のある１つの区間において推定した周期関数を繰返し用いることで、他の区間も含めたトルク集合全体の補償を実施可能となる。 As described above, in the torque compensating device according to the first embodiment, the torque disturbance has periodicity, and by repeatedly using the periodic function estimated in one section of one cycle unit, the other sections can also be used. Compensation for the entire torque set including it becomes possible.
　また上記では、推定部３が、一区間を用いて補償関数を推定する場合を示した。しかしながら、これに限らず、推定部３は、複数区間を用いて補償関数を推定してもよい。この場合、推定部３は、対象とする区間における誤差関数を求め、その総和を最小化するような補償関数を推定する。この際、推定部３は、各区間の誤差関数に任意の重みをかけることで、制御に重要な区間をより重点的に補償するような推定も可能である。これにより、制御性能の向上が見込める。 Further, in the above, the case where the estimation unit 3 estimates the compensation function using one section is shown. However, not limited to this, the estimation unit 3 may estimate the compensation function using a plurality of sections. In this case, the estimation unit 3 obtains an error function in the target section and estimates a compensation function that minimizes the sum. At this time, the estimation unit 3 can make an estimation that compensates the section important for control more intensively by applying an arbitrary weight to the error function of each section. As a result, improvement in control performance can be expected.
　なお、補償関数として、ロボットアームの稼動条件に応じて複数の関数を使い分けて適用することも可能である。これにより、実施の形態１に係るトルク補償装置は、稼動条件によるトルク外乱の変動への対応が可能となる。 It is also possible to use and apply a plurality of functions as compensation functions according to the operating conditions of the robot arm. As a result, the torque compensating device according to the first embodiment can cope with fluctuations in torque disturbance due to operating conditions.
　以上のように、この実施の形態１によれば、トルク補償装置は、ロボットアームに取付けられたトルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際のロボットアームの関節角を取得するデータ取得部１と、回転機構を持つ減速機に起因するトルク外乱の関節角に対する周期関数である補償関数、及びデータ取得部１により取得された関節角に基づいて、当該関節角におけるトルク外乱による誤差を補償するためのトルク補償値を算出する補償値算出部４２と、データ取得部１により取得されたトルクと、補償値算出部４２により算出されたトルク補償値との残差を補償後トルクとして算出する残差算出部４３と、残差算出部４３により算出された補償後トルクを示すデータを出力する出力部４４とを備えた。これにより、実施の形態１に係るトルク補償装置は、機械的な機構に依らず、トルク外乱による誤差を補償可能となる。 As described above, according to the first embodiment, the torque compensator determines the torque detected by the torque sensor attached to the robot arm and the joint angle of the robot arm when the torque sensor detects the torque. Torque at the joint angle based on the data acquisition unit 1 to be acquired, the compensation function which is a periodic function for the joint angle of torque disturbance caused by the speed reducer having a rotation mechanism, and the joint angle acquired by the data acquisition unit 1. Compensates for the residual between the compensation value calculation unit 42 that calculates the torque compensation value for compensating for the error due to the disturbance, the torque acquired by the data acquisition unit 1, and the torque compensation value calculated by the compensation value calculation unit 42. A residual calculation unit 43 calculated as a rear torque and an output unit 44 that outputs data indicating the compensated torque calculated by the residual calculation unit 43 are provided. As a result, the torque compensating device according to the first embodiment can compensate for an error due to torque disturbance regardless of the mechanical mechanism.
実施の形態２．
　実施の形態１では、補償関数として適用可能な関数を限定していない。これに対し、実施の形態２では、補償関数として用いる関数を正弦関数に限定している。正弦関数はそれ自体が周期関数であり、その重ね合わせで任意の周期関数を表現できるため、補償関数として好適である。また、正弦関数は周期関数であるため、実施の形態１のように周期毎に区切らなくても関数推定が可能であり、実施の形態２では複数区間を対象として一括で関数推定を行う。また、推定部３は、関数推定の前に初期値の算出を行うことが好ましい。これは、正弦関数を表すパラメータの推定を効率化するためである。
　図６は実施の形態２に係るトルク補償装置の構成例を示す図である。図６に示す実施の形態２に係るトルク補償装置は、図１に示す実施の形態１に係るトルク補償装置に対し、初期値算出部３３を追加している。その他の構成は同様であり、同一の符号を付して異なる部分についてのみ説明を行う。 Embodiment 2.
In the first embodiment, the function applicable as the compensation function is not limited. On the other hand, in the second embodiment, the function used as the compensation function is limited to the sine function. The sine function itself is a periodic function, and an arbitrary periodic function can be expressed by superimposing the sine function, so that it is suitable as a compensation function. Further, since the sine function is a periodic function, it is possible to estimate the function without dividing it into each period as in the first embodiment, and in the second embodiment, the function estimation is performed collectively for a plurality of sections. Further, it is preferable that the estimation unit 3 calculates the initial value before the function estimation. This is to streamline the estimation of the parameters representing the sine function.
FIG. 6 is a diagram showing a configuration example of the torque compensating device according to the second embodiment. The torque compensating device according to the second embodiment shown in FIG. 6 has an initial value calculation unit 33 added to the torque compensating device according to the first embodiment shown in FIG. Other configurations are the same, and only different parts will be described with the same reference numerals.
　初期値算出部３３は、前処理部２による前処理後のデータに基づいて、正弦関数を表すパラメータの初期値を算出する。初期値算出部３３は、図６に示すように、初期振幅算出部３３１、初期周波数算出部３３２及び初期位相算出部３３３を有している。 The initial value calculation unit 33 calculates the initial value of the parameter representing the sine function based on the data after the preprocessing by the preprocessing unit 2. As shown in FIG. 6, the initial value calculation unit 33 includes an initial amplitude calculation unit 331, an initial frequency calculation unit 332, and an initial phase calculation unit 333.
　初期振幅算出部３３１は、前処理部２による前処理後のデータに基づいて、正弦関数を表すパラメータである振幅の初期値を算出する。
　初期周波数算出部３３２は、前処理部２による前処理後のデータに基づいて、正弦関数を表すパラメータである周波数の初期値を算出する。 The initial amplitude calculation unit 331 calculates the initial value of the amplitude, which is a parameter representing the sine function, based on the data after the preprocessing by the preprocessing unit 2.
The initial frequency calculation unit 332 calculates the initial value of the frequency, which is a parameter representing the sine function, based on the data after the preprocessing by the preprocessing unit 2.
　なお図６では、初期振幅算出部３３１の後段に初期周波数算出部３３２が設けられた場合を示している。しかしながら、これに限らず、初期振幅算出部３３１の前段に初期周波数算出部３３２が設けられていてもよい。 Note that FIG. 6 shows a case where the initial frequency calculation unit 332 is provided after the initial amplitude calculation unit 331. However, the present invention is not limited to this, and the initial frequency calculation unit 332 may be provided in front of the initial amplitude calculation unit 331.
　初期位相算出部３３３は、初期振幅算出部３３１により算出された振幅の初期値及び初期周波数算出部３３２により算出された周波数の初期値に基づいて、正弦関数を表すパラメータである位相の初期値を算出する。 The initial phase calculation unit 333 calculates the initial value of the phase, which is a parameter representing a sine function, based on the initial value of the amplitude calculated by the initial amplitude calculation unit 331 and the initial value of the frequency calculated by the initial frequency calculation unit 332. calculate.
　なお、関数推定部３１は、前処理部２による前処理後のデータ及び初期値算出部３３により算出された初期値に基づいて、補償関数を推定する。補償関数は、正弦関数の重ね合わせにより表される。 The function estimation unit 31 estimates the compensation function based on the data after the preprocessing by the preprocessing unit 2 and the initial value calculated by the initial value calculation unit 33. The compensation function is represented by the superposition of sine functions.
　なお、初期値算出部３３は、関数推定部３１による関数推定を効率化するための手段であり、トルク補償装置に必須の構成ではない。また、トルク補償装置に初期値算出部３３が設けられていない場合には、正弦関数を表すパラメータの初期値は乱数等の任意の値とする。 Note that the initial value calculation unit 33 is a means for improving the efficiency of function estimation by the function estimation unit 31, and is not an essential configuration for the torque compensation device. Further, when the torque compensation device is not provided with the initial value calculation unit 33, the initial value of the parameter representing the sine function is an arbitrary value such as a random number.
　次に、図６に示す実施の形態２に係るトルク補償装置による推定処理例について、図７を参照しながら説明する。
　図６に示す実施の形態２に係るトルク補償装置による推定処理例では、図７に示すように、まず、データ取得部１は、トルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際のロボットアームの関節角を取得する（ステップＳＴ７０１）。　　　 Next, an example of estimation processing by the torque compensator according to the second embodiment shown in FIG. 6 will be described with reference to FIG. 7.
In the estimation processing example by the torque compensating device according to the second embodiment shown in FIG. 6, as shown in FIG. 7, first, the data acquisition unit 1 detects the torque detected by the torque sensor and the torque sensor detects the torque. The joint angle of the robot arm at the time of this is acquired (step ST701).
　次いで、前処理部２は、データ取得部１による取得結果を示すデータに対して前処理を行う（ステップＳＴ７０２）。
　ステップＳＴ７０１，７０２における処理は、実施の形態１におけるステップＳＴ２０１，２０２における処理と同様である。 Next, the preprocessing unit 2 performs preprocessing on the data indicating the acquisition result by the data acquisition unit 1 (step ST702).
The processing in steps ST701 and 702 is the same as the processing in steps ST201 and 202 in the first embodiment.
　次いで、初期値算出部３３は、前処理部２による前処理後のデータに基づいて、正弦関数を表すパラメータである初期値を算出する（ステップＳＴ７０３）。以下、初期値算出部３３による初期値算出動作例について、図８を参照しながら説明する。 Next, the initial value calculation unit 33 calculates the initial value, which is a parameter representing the sine function, based on the data after the preprocessing by the preprocessing unit 2 (step ST703). Hereinafter, an example of the initial value calculation operation by the initial value calculation unit 33 will be described with reference to FIG.
　初期値算出部３３による初期値算出動作例では、図８に示すように、まず、初期振幅算出部３３１は、前処理部２による前処理後のデータに基づいて、正弦関数を表すパラメータである振幅の初期値を算出する（ステップＳＴ８０１）。 In the initial value calculation operation example by the initial value calculation unit 33, first, as shown in FIG. 8, the initial amplitude calculation unit 331 is a parameter representing a sine function based on the data after the preprocessing by the preprocessing unit 2. The initial value of the amplitude is calculated (step ST801).
　この際、初期振幅算出部３３１は、まず、前処理部２による前処理後のデータが示すトルクに対し、離散フーリエ変換を行う。ここでは、離散フーリエ変換として、離散フーリエ変換を高速に行う高速フーリエ変換（ＦＦＴ：Ｆａｓｔ　Ｆｏｕｒｉｅｒ　Ｔｒａｎｓｆｏｒｍ）を用いるものとする。このＦＦＴにより、トルクの波形を、関節角領域から周波数領域で表すことができる。周期関数は単純な正弦関数又は余弦関数の重ね合わせで表現できることは公知であり、トルク外乱に周期性が有ることを踏まえると、トルクを周波数領域で表した結果からその関数を推定可能であるとわかる。ここでは、一例として、三角関数のうちの正弦関数を用いるものとする。 At this time, the initial amplitude calculation unit 331 first performs a discrete Fourier transform on the torque indicated by the data after the preprocessing by the preprocessing unit 2. Here, as the discrete Fourier transform, it is assumed that a fast Fourier transform (FFT: Fast Fourier Transform) that performs the discrete Fourier transform at high speed is used. With this FFT, the torque waveform can be represented in the frequency domain from the joint angle domain. It is known that the periodic function can be expressed by superimposing a simple sine function or cosine function, and considering that the torque disturbance has periodicity, it is possible to estimate the function from the result of expressing the torque in the frequency domain. Understand. Here, as an example, it is assumed that the sine function among the trigonometric functions is used.
　そして、初期振幅算出部３３１は、周波数領域で表したトルクに基づいて、ピークの絶対値を取ることで振幅の初期値（ａｉｎｉｔ）を求める。周波数領域におけるピークの絶対値の大小は、その関数における影響の大小であると言えるため、絶対値の大きい周波数から補償に適用することが効率的である。すなわち、初期振幅算出部３３１が、振幅の初期値として選ぶのは、最も大きいピークの絶対値となる。 Then, the initial amplitude calculation unit 331 obtains the initial value (a init ) of the amplitude by taking the absolute value of the peak based on the torque expressed in the frequency domain. Since it can be said that the magnitude of the absolute value of the peak in the frequency domain is the magnitude of the influence on the function, it is efficient to apply the compensation from the frequency having the largest absolute value. That is, the initial amplitude calculation unit 331 selects the absolute value of the largest peak as the initial value of the amplitude.
　また、初期周波数算出部３３２は、前処理部２による前処理後のデータに基づいて、正弦関数を表すパラメータである周波数の初期値を算出する（ステップＳＴ８０２）。
　この際、初期周波数算出部３３２は、まず、前処理部２による前処理後のデータが示すトルクに対し、離散フーリエ変換を行う。ここでは、離散フーリエ変換として高速フーリエ変換（ＦＦＴ）を用いるものとする。そして、初期周波数算出部３３２は、周波数領域で表したトルクに基づいて、ピークの絶対値を持つ周波数を、周波数の初期値（ｆｉｎｉｔ）とする。 Further, the initial frequency calculation unit 332 calculates the initial value of the frequency, which is a parameter representing the sine function, based on the data after the preprocessing by the preprocessing unit 2 (step ST802).
At this time, the initial frequency calculation unit 332 first performs a discrete Fourier transform on the torque indicated by the data after the preprocessing by the preprocessing unit 2. Here, it is assumed that the fast Fourier transform (FFT) is used as the discrete Fourier transform. Then, the initial frequency calculation unit 332 sets the frequency having the absolute value of the peak as the initial value ( init ) of the frequency based on the torque expressed in the frequency domain.
　次いで、初期位相算出部３３３は、初期振幅算出部３３１により算出された振幅の初期値及び初期周波数算出部３３２により算出された周波数の初期値に基づいて、正弦関数を表すパラメータである位相の初期値を算出する（ステップＳＴ８０３）。 Next, the initial phase calculation unit 333 is the initial phase, which is a parameter representing a sine function, based on the initial value of the amplitude calculated by the initial amplitude calculation unit 331 and the initial value of the frequency calculated by the initial frequency calculation unit 332. Calculate the value (step ST803).
　この際、初期位相算出部３３３は、初期振幅算出部３３１により算出された振幅の初期値と、初期周波数算出部３３２により算出された周波数の初期値とを用いて、初期周期関数（ｌ（ｘ））を生成し、その初期周期関数を用いて位相の初期値（ｐｉｎｉｔ）を求める。初期周期関数は、補償に用いる周期関数として正弦関数を用いることを想定しているため、下式（５）で表される。
ｌ（ｘ）＝ａｉｎｉｔ×ｓｉｎ（２π×ｆｉｎｉｔ×ｘ）　（５） At this time, the initial phase calculation unit 333 uses the initial value of the amplitude calculated by the initial amplitude calculation unit 331 and the initial value of the frequency calculated by the initial frequency calculation unit 332, and uses the initial periodic function (l (x). )) Is generated, and the initial value ( pinit ) of the phase is obtained by using the initial periodic function. The initial periodic function is expressed by the following equation (5) because it is assumed that a sine function is used as the periodic function used for compensation.
l (x) = a init × sin (2π × init × x) (5)
　初期位相算出部３３３は、トルクが持つ周波数成分のうちの影響が大きいとされる周波数とその振幅を用いて、初期周期関数を生成する。そのため、初期位相算出部３３３は、初期周期関数とトルクの位相差を求め、その位相差を位相の初期値とすればよい。初期位相算出部３３３は、初期周期関数とトルクとの相互相関関数によって位相差を求めることができる。すなわち、上記相互相関関数が最大となるときの位相差が、初期周期関数とトルクの波形が最もよく重なるときの位相差であり、初期位相算出部３３３はこれを位相の初期値とする。 The initial phase calculation unit 333 generates an initial periodic function using the frequency that is considered to have a large influence on the frequency component of the torque and its amplitude. Therefore, the initial phase calculation unit 333 may obtain the phase difference between the initial periodic function and the torque, and use the phase difference as the initial value of the phase. The initial phase calculation unit 333 can obtain the phase difference by the cross-correlation function of the initial periodic function and the torque. That is, the phase difference when the cross-correlation function is maximized is the phase difference when the initial periodic function and the torque waveform overlap most, and the initial phase calculation unit 333 sets this as the initial value of the phase.
　再び図７に示すトルク補償装置による推定処理例の説明に戻り、次いで、関数推定部３１は、前処理部２による前処理後のデータ及び初期値算出部３３により算出された初期値に基づいて、補償関数を推定する（ステップＳＴ７０４）。 Returning to the description of the estimation processing example by the torque compensator shown in FIG. 7, the function estimation unit 31 then returns to the data after the preprocessing by the preprocessing unit 2 and the initial value calculated by the initial value calculation unit 33. , Estimate the compensation function (step ST704).
　この際、関数推定部３１は、まず、初期値算出部３３により算出された初期値（振幅の初期値、周波数の初期値及び位相の初期値）を用いて、回転機構を持つ減速機に起因するトルク外乱による誤差を補償するための正弦関数として適切なパラメータ（振幅（ａ）、周波数（ｆ）及び位相（ｐ））を推定する。 At this time, the function estimation unit 31 first uses the initial values (amplitude initial value, frequency initial value, and phase initial value) calculated by the initial value calculation unit 33, and is caused by a speed reducer having a rotation mechanism. Estimate appropriate parameters (amplitude (a), frequency (f) and phase (p)) as a sine function to compensate for errors due to torque disturbances.
　前提として、上記トルク外乱による誤差を補償するための正弦関数として適切なパラメータとは、下式（６）を示す関数（Ｌ（ｘ））を最小化するものである。ここで、関節角（ｘ）及びトルク（ｔ）はＮ個のデータ点を含むものであり、ｔｊは関節角（ｘｊ）に対するトルクの値を示している。
Figure JPOXMLDOC01-appb-I000001
As a premise, an appropriate parameter as a sine function for compensating for the error due to the torque disturbance is to minimize the function (L (x)) showing the following equation (6). Here, the joint angle (x) and the torque (t) include N data points, and t j indicates the value of the torque with respect to the joint angle (x j ).
Figure JPOXMLDOC01-appb-I000001
　関数推定部３１は、上記関数を最小化するようなパラメータを最小二乗法によって推定する。関数推定部３１は、上記推定の際の初期値として、初期値算出部３３により算出された初期値（振幅の初期値、周波数の初期値及び位相の初期値）を用いる。これにより、実施の形態２に係るトルク補償装置では、乱数等で定めた初期値に対して解に到達するまでの推定時間を短縮可能である。 The function estimation unit 31 estimates the parameters that minimize the above function by the least squares method. The function estimation unit 31 uses the initial values (initial value of amplitude, initial value of frequency, and initial value of phase) calculated by the initial value calculation unit 33 as the initial value at the time of the above estimation. As a result, in the torque compensating device according to the second embodiment, it is possible to shorten the estimated time until the solution is reached with respect to the initial value determined by a random number or the like.
　その後、関数推定部３１は、上記処理を繰返し、トルクに含まれる各周波数成分についてそれぞれ補償する正弦関数のパラメータを推定し、複数の正弦関数を求める。なお、関数推定部３１は、推定の際に複数の正弦関数を用いてそれらのパラメータを同時に推定してもよく、その推定を更に繰返して正弦関数の数を増やしてもよい。 After that, the function estimation unit 31 repeats the above process, estimates the parameters of the trigonometric function that compensates for each frequency component included in the torque, and obtains a plurality of trigonometric functions. The function estimation unit 31 may simultaneously estimate those parameters by using a plurality of sine functions at the time of estimation, or may further repeat the estimation to increase the number of sine functions.
　その後、関数推定部３１は、下式（７）から、任意の関節角（ｘ（ハット））におけるトルク外乱による誤差を補償するためのトルク補償値（ｙ（ハット））を求める。式（７）において、ｍは関数推定部３１が推定に用いた正弦関数の数を示し、ａｉはｉ番目の正弦関数を表すパラメータである振幅を示し、ｆｉはｉ番目の正弦関数を表すパラメータである周波数を示し、ｐｉはｉ番目の正弦関数を表すパラメータである位相を示している。 After that, the function estimation unit 31 obtains a torque compensation value (y (hat)) for compensating for an error due to torque disturbance at an arbitrary joint angle (x (hat)) from the following equation (7). In the formula (7), m represents a number of sinusoidal function estimator 31 is used to estimate, a i denotes the amplitude is a parameter representing the i-th sinusoidal, f i is the i-th sinusoidal It represents represents frequency is a parameter, p i denotes the phase is a parameter representing the i-th sine function.
Figure JPOXMLDOC01-appb-I000002
Figure JPOXMLDOC01-appb-I000002
　次いで、出力部３２は、関数推定部３１により推定された補償関数を示すデータを出力する（ステップＳＴ７０５）。 Next, the output unit 32 outputs data indicating the compensation function estimated by the function estimation unit 31 (step ST705).
　このように、実施の形態２に係るトルク補償装置では、トルク外乱が関節角の周期関数であることを前提として正弦関数による近似を行うことで、その周波数成分の補償を行うことができる。これを繰返し、トルクに含まれる各周波数成分を補償する複数の正弦関数の重ね合わせを補償関数として求めることで、トルク全体の補償を行うことができる。補償に用いる正弦関数は、トルク外乱との誤差が最小化されるようなパラメータによって生成されることが望ましく、トルク補償装置は、予め初期値を算出しておくことで効率的に補償関数を推定可能である。 As described above, in the torque compensating device according to the second embodiment, the frequency component can be compensated by performing approximation by a sine function on the premise that the torque disturbance is a periodic function of the joint angle. By repeating this and obtaining the superposition of a plurality of sinusoidal functions that compensate for each frequency component included in the torque as a compensation function, the entire torque can be compensated. It is desirable that the sine function used for compensation is generated by parameters that minimize the error with torque disturbance, and the torque compensation device efficiently estimates the compensation function by calculating the initial value in advance. It is possible.
　なお、本願発明はその発明の範囲内において、各実施の形態の自由な組合わせ、或いは各実施の形態の任意の構成要素の変形、若しくは各実施の形態において任意の構成要素の省略が可能である。
　また上記では、ロボットアームが、回転関節を持ったロボットアームである場合を例に示した。しかしながら、これに限らず、ロボットアームは、トルクセンサが取付けられた回転駆動機構を持ったロボットアームであればよく、例えば、スカラ型ロボットアームのような複数軸構成を持ったロボットアーム、パラレルリンク型ロボットアーム、又は一軸のみを持つロボットアームでもよい。 In the present invention, within the scope of the invention, it is possible to freely combine each embodiment, modify any component of each embodiment, or omit any component in each embodiment. is there.
Further, in the above, the case where the robot arm is a robot arm having a rotary joint is shown as an example. However, the robot arm is not limited to this, and may be any robot arm having a rotation drive mechanism to which a torque sensor is attached. For example, a robot arm having a multi-axis configuration such as a scalar type robot arm, or a parallel link. A type robot arm or a robot arm having only one axis may be used.
　この発明に係るトルク補償装置は、機械的な機構に依らず、トルク外乱による誤差を補償可能となり、トルクセンサが取付けられたロボットアームに対してトルク外乱による誤差の補償を行うトルク補償装置に用いるのに適している。 The torque compensating device according to the present invention can compensate for an error due to torque disturbance regardless of a mechanical mechanism, and is used as a torque compensating device for compensating for an error due to torque disturbance to a robot arm to which a torque sensor is attached. Suitable for.
１　データ取得部
２　前処理部
３　推定部
４　補償部
３１　関数推定部
３２　出力部
３３　初期値算出部
４１　関数読込部
４２　補償値算出部
４３　残差算出部
４４　出力部
３３１　初期振幅算出部
３３２　初期周波数算出部
３３３　初期位相算出部 1 Data acquisition unit 2 Preprocessing unit 3 Estimating unit 4 Compensation unit 31 Function estimation unit 32 Output unit 33 Initial value calculation unit 41 Function reading unit 42 Compensation value calculation unit 43 Residual calculation unit 44 Output unit 331 Initial amplitude calculation unit 332 Initial Frequency calculation unit 333 Initial phase calculation unit

Claims (5)

  1. 　ロボットアームに取付けられたトルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際の当該ロボットアームの関節角を取得するデータ取得部と、
    　回転機構を持つ減速機に起因するトルク外乱の関節角に対する周期関数である補償関数、及び前記データ取得部により取得された関節角に基づいて、当該関節角におけるトルク外乱による誤差を補償するためのトルク補償値を算出する補償値算出部と、
    　前記データ取得部により取得されたトルクと、前記補償値算出部により算出されたトルク補償値との残差を補償後トルクとして算出する残差算出部と、
    　前記残差算出部により算出された補償後トルクを示すデータを出力する出力部と
    　を備えたトルク補償装置。     A data acquisition unit that acquires the torque detected by the torque sensor attached to the robot arm and the joint angle of the robot arm when the torque sensor detects the torque.
    To compensate for the error due to torque disturbance at the joint angle based on the compensation function which is a periodic function for the joint angle of torque disturbance caused by the speed reducer having a rotation mechanism and the joint angle acquired by the data acquisition unit. Compensation value calculation unit that calculates torque compensation value,
    A residual calculation unit that calculates the residual between the torque acquired by the data acquisition unit and the torque compensation value calculated by the compensation value calculation unit as the torque after compensation.
    A torque compensation device including an output unit that outputs data indicating the compensated torque calculated by the residual calculation unit.
  2. 　前記データ取得部による取得結果に基づいて、前記減速機に起因するトルク外乱の関節角に対する周期関数である補償関数を推定する関数推定部を備え、
    　前記補償値算出部は、前記関数推定部により推定された補償関数を用いてトルク補償値を算出する
    　ことを特徴とする請求項１記載のトルク補償装置。     A function estimation unit that estimates a compensation function that is a periodic function for the joint angle of torque disturbance caused by the speed reducer based on the acquisition result by the data acquisition unit is provided.
    The torque compensation device according to claim 1, wherein the compensation value calculation unit calculates a torque compensation value using a compensation function estimated by the function estimation unit.
  3. 　前記関数推定部は、正弦関数の重ね合わせにより表された補償関数を推定する
    　ことを特徴とする請求項２記載のトルク補償装置。     The torque compensation device according to claim 2, wherein the function estimation unit estimates a compensation function represented by superposition of trigonometric functions.
  4. 　前記データ取得部による取得結果に基づいて、正弦関数を表すパラメータの初期値を算出する初期値算出部を備え、
    　前記関数推定部は、前記データ取得部による取得結果及び前記初期値算出部により算出された初期値に基づいて、補償関数を推定する
    　ことを特徴とする請求項３記載のトルク補償装置。     An initial value calculation unit for calculating the initial value of a parameter representing a sine function based on the acquisition result by the data acquisition unit is provided.
    The torque compensation device according to claim 3, wherein the function estimation unit estimates a compensation function based on an acquisition result by the data acquisition unit and an initial value calculated by the initial value calculation unit.
  5. 　ロボットアームに取付けられたトルクセンサにより検出されたトルク、及び当該トルクセンサがトルクを検出した際の当該ロボットアームの関節角を取得するデータ取得ステップと、
    　回転機構を持つ減速機に起因するトルク外乱の関節角に対する周期関数である補償関数、及び前記データ取得ステップにおいて取得した関節角に基づいて、当該関節角におけるトルク外乱による誤差を補償するためのトルク補償値を算出する補償値算出ステップと、
    　前記データ取得ステップにおいて取得したトルクと、前記補償値算出ステップにおいて算出したトルク補償値との残差を補償後トルクとして算出する残差算出ステップと、
    　前記残差算出ステップにおいて算出した補償後トルクを示すデータを出力する出力ステップと
    　を有するトルク補償方法。     A data acquisition step for acquiring the torque detected by the torque sensor attached to the robot arm and the joint angle of the robot arm when the torque sensor detects the torque.
    Torque to compensate for the error due to torque disturbance at the joint angle based on the compensation function which is a periodic function for the joint angle of torque disturbance caused by the speed reducer having a rotation mechanism and the joint angle acquired in the data acquisition step. Compensation value calculation step to calculate compensation value and
    A residual calculation step of calculating the residual between the torque acquired in the data acquisition step and the torque compensation value calculated in the compensation value calculation step as the post-compensation torque.
    A torque compensation method including an output step for outputting data indicating the compensated torque calculated in the residual calculation step.
PCT/JP2020/024788 2019-07-08 2020-06-24 Torque compensation device and torque compensation method WO2021006038A1 (en)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2019-126646 2019-07-08
JP2019126646A JP2021010983A (en) 2019-07-08 2019-07-08 Torque compensation device and torque compensation method

Publications (1)

Publication Number Publication Date
WO2021006038A1 true WO2021006038A1 (en) 2021-01-14

Family

ID=74114764

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2020/024788 WO2021006038A1 (en) 2019-07-08 2020-06-24 Torque compensation device and torque compensation method

Country Status (2)

Country Link
JP (1) JP2021010983A (en)
WO (1) WO2021006038A1 (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102628668B1 (en) * 2021-11-30 2024-01-23 두산로보틱스 주식회사 Robot arm gravity compensating device and method

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0887330A (en) * 1994-09-20 1996-04-02 Fujitsu Ltd Automatic controller
JPH10243676A (en) * 1997-02-21 1998-09-11 Matsushita Electric Ind Co Ltd Controller and control method
JP2009285824A (en) * 2008-05-30 2009-12-10 Samsung Electronics Co Ltd Device and method for controlling robot manipulator
WO2010090059A1 (en) * 2009-02-03 2010-08-12 オリンパスメディカルシステムズ株式会社 Manipulator
JP2013066965A (en) * 2011-09-21 2013-04-18 Toshiba Corp Robot controller, disturbance determination method, and actuator control method
CN107538494A (en) * 2016-06-29 2018-01-05 沈阳新松机器人自动化股份有限公司 A kind of robot control method and system based on torque sensor and encoder

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0887330A (en) * 1994-09-20 1996-04-02 Fujitsu Ltd Automatic controller
JPH10243676A (en) * 1997-02-21 1998-09-11 Matsushita Electric Ind Co Ltd Controller and control method
JP2009285824A (en) * 2008-05-30 2009-12-10 Samsung Electronics Co Ltd Device and method for controlling robot manipulator
WO2010090059A1 (en) * 2009-02-03 2010-08-12 オリンパスメディカルシステムズ株式会社 Manipulator
JP2013066965A (en) * 2011-09-21 2013-04-18 Toshiba Corp Robot controller, disturbance determination method, and actuator control method
CN107538494A (en) * 2016-06-29 2018-01-05 沈阳新松机器人自动化股份有限公司 A kind of robot control method and system based on torque sensor and encoder

Also Published As

Publication number Publication date
JP2021010983A (en) 2021-02-04

Similar Documents

Publication Publication Date Title
JP4676551B1 (en) Motor control device having cogging torque correction amount calculation function
US8004434B2 (en) RD converter and angle detecting apparatus
JP4418390B2 (en) Three-dimensional shape processing apparatus, curved surface generation program and method
JP2018534576A5 (en)
WO2021006038A1 (en) Torque compensation device and torque compensation method
JP6312920B2 (en) Measurement method of vibration displacement using state variation principle
KR20160066509A (en) Correction table generating apparatus, encoder, and correction table generating method
JP4920612B2 (en) Actuator angle transmission error compensation method
JP2014136260A (en) Control device
Gasparetto et al. Validation of minimum time-jerk algorithms for trajectory planning of industrial robots
TW201523183A (en) System and method for compensating precision of rotating tables
JP6245858B2 (en) Control device
JP7338960B2 (en) Servo control device and servo control method
JP5747752B2 (en) Posture estimation device, posture estimation method, posture estimation program
Novo et al. Non linear simulation of a wave energy converter with multiple degrees of freedom using a harmonic balance method
JP5079346B2 (en) Waveform correction apparatus and waveform correction method
Sracic et al. Identifying parameters of multi-degree-of-freedom nonlinear structural dynamic systems using linear time periodic approximations
JP6041325B2 (en) Signal processing apparatus, signal processing method, and program
JP2005224051A (en) Motor controller
Gu et al. Study on torsional vibration of a harmonic driver based on time-varying stiffness caused by manufacturing error
Borboni et al. Fast kinematic model of a seven-bar linkage with a single compliant link
Du et al. Comparison of three methods of length compensation in a parallel kinematic and their equivalence conditions
JP2020069616A (en) Motor driving device, robot driving system and motor driving program
Dakel et al. Bifurcation analysis of a non-linear on-board rotor-bearing system
JP2016070706A (en) Calculation method, measurement device, program, and information processing device

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 20837105

Country of ref document: EP

Kind code of ref document: A1

122 Ep: pct application non-entry in european phase

Ref document number: 20837105

Country of ref document: EP

Kind code of ref document: A1