JP7464391B2 - Mechanical system with trajectory estimation using motor encoders and sensors - Google Patents

Description

The present invention relates to a mechanical system, and in particular to a mechanical system that performs trajectory estimation using a motor encoder and a sensor.

A method has been proposed for controlling vibrations at the tip of machines such as industrial robots and machine tools by attaching a sensor to the tip of the machine to measure vibrations during operation.

Patent document 1 discloses that vibrations of an arm are detected by an arm root position sensor attached to the base of the arm and an accelerometer attached to the tip of the arm. It describes how a command to move the arm to a target position is calculated based on values obtained by comparing an arm root position detection signal obtained by the arm root position sensor, an arm tip speed detection signal obtained by the accelerometer, an arm tip speed set value, and an arm root position set value, and the result is sent to an actuator as a control signal.

Patent document 2 discloses that a learning control unit is provided that performs learning to calculate a learning correction amount in order to bring the controlled position of the robot mechanism detected by a sensor closer to a target trajectory or target position, and that the learning control unit performs learning to calculate the learning correction amount while increasing the speed override multiple times until the maximum speed override is reached.

Patent document 3 discloses that the learning control unit is equipped with a position deviation estimation unit that estimates the low-frequency components of the position deviation from information from the motor encoder, and estimates the high-frequency components of the position deviation from information from the sensor.

Patent Document 4 discloses that when a delay occurs in the transmission of a signal from a wireless sensor, in order to correct the sensor signal, first time series data of the acceleration of the arm tip corresponding to the sensor signal is obtained, and second time series data of the acceleration of the arm tip is obtained by forward transforming and second-order differentiating the command values of the motors of each axis, and the delay time of the first time series data relative to the second time series data is calculated according to the degree of correlation between the first time series data and the second time series data, and the first time series data is corrected using the calculated delay time.

Japanese Patent Application Laid-Open No. 8-47882 JP 2011-167817 A JP 2019-181610 A JP 2015-163416 A

Sensors such as acceleration sensors, gyro sensors, and inertial sensors have a problem in that estimation errors in low-frequency components accumulate when estimating position from the sensor's speed and acceleration data. This estimation error can be removed with a high-pass filter, but the low-frequency components of the trajectory of the machine tip are also removed, making it impossible to estimate the trajectory deviation between the target trajectory and the actual trajectory. As a solution to this problem, the low-frequency components of the position data from the motor encoder can be used to improve the accuracy of trajectory estimation.

However, the trajectory of the machine tip estimated by the motor encoder does not take into account the delay time from the motor encoder to the machine tip caused by the machine mechanism and tool, such as deflection and backlash of the machine mechanism and tool. Therefore, the estimation error can be large under high loads and high acceleration when deflection is large, and when each axis is rotating forward and backward. In addition, this delay time changes from moment to moment depending on the rigidity, speed, acceleration, load, and posture of the machine mechanism and tool, making it difficult to estimate accurately.

Therefore, there is a demand for technology that can correct the delay time from the motor encoder to the tip of the machine, which is caused by the machine mechanism and tools.

One aspect of the present disclosure provides a mechanical system that estimates a trajectory of a machine using a sensor provided at a tip of the machine and a motor encoder provided at a machine mechanism, the mechanical system comprising: a delay time calculation unit that calculates a delay time from the motor encoder to the tip of the machine based on a degree of correlation between a sensor time series signal estimated or acquired by the sensor and an encoder time series signal acquired or estimated by the motor encoder; a signal waveform correction unit that corrects the waveform of the encoder time series signal based on the calculated delay time; and a trajectory estimation unit that estimates an actual trajectory of the tip of the machine by subtracting the corrected encoder time series signal from the sensor time series signal, removing low frequency components from the sensor time series signal obtained by subtracting the encoder time series signal, and adding the encoder time series signal to the sensor time series signal from which the low frequency components have been removed.

According to one aspect of the present disclosure, the waveform of the encoder time series signal is corrected using a sensor time series signal that includes a delay time from the motor encoder to the tip of the machine, so that an encoder time series signal that takes the delay time into account can be generated. As a result, the actual trajectory of the tip of the machine can be accurately estimated based on the sensor time series signal and the encoder time series signal.

1 is a block diagram showing a schematic configuration of a mechanical system according to an embodiment; 11 is a diagram showing an example of a delay time caused by at least one of a mechanical mechanism and a tool; FIG. FIG. 13 illustrates an example of an encoder estimated position corrected based on a delay time. FIG. 4 is a block diagram showing an example of pre-processing of a sensor time-series signal and a motor encoder time-series signal. 11 is a graph showing an example of a waveform before an offset error of a sensor is removed. 11 is a graph showing an example of a correlation before removing an offset error of a sensor. 11 is a graph showing an example of a waveform after removing an offset error of a sensor. 13 is a graph showing an example of correlation after removing sensor offset errors. 11 is a graph showing an example of calculating a delay time without dividing into sections. 11 is a graph showing an example of calculating a delay time for each section. FIG. 4 is a block diagram showing an example of a trajectory estimation unit. FIG. 11 is a block diagram showing a schematic configuration of a mechanical system according to another embodiment.

The embodiments of the present disclosure will be described in detail below with reference to the accompanying drawings. In each drawing, the same or similar components are given the same or similar reference numerals. Furthermore, the embodiments described below do not limit the technical scope of the invention described in the claims and the meaning of the terms.

Figure 1 shows a schematic configuration of a machine system 1 in this embodiment. The machine system 1 is a standalone system and includes a machine 10 and a control device 20 that controls the operation of the machine 10. The machine 10 is, for example, a multi-joint robot, but may be other industrial machines such as a single-joint robot, a single-axis or multi-axis machine tool, or a construction machine. The machine 10 includes a machine mechanism unit 11 equipped with a motor (not shown) and a tool 12 attached to the tip of the machine mechanism unit 11. The machine mechanism unit 11 is, for example, a robot arm, but may be a ball screw, rail, or the like in other industrial machines. The tool 12 is, for example, a sealing tool, but may be a welding tool, hand, or the like. The control device 20 is a computer device equipped with a processor such as a central processing unit (CPU), an application specific integrated circuit (ASIC), or a field-programmable gate array (FPGA). The machine 10 and the control device 20 are connected to each other so that they can communicate with each other via wires or wirelessly.

The control device 20 estimates the actual trajectory of the machine tip 15 using the sensor 13 provided at the machine tip 15 and the motor encoder 14 provided at the machine mechanism unit 11. The machine tip 15 refers to, for example, the vicinity of the tip of the tool 12, but may be other control parts that are the subject of position control. The sensor 13 is, for example, an acceleration sensor, a gyro sensor, an inertial sensor, etc., but may be other sensors capable of detecting vibrations of the machine tip 15. The sensor 13 outputs sensor time series signals such as the position, speed, acceleration, and load of the machine tip 15. The motor encoder 14 is, for example, a rotary encoder, but may instead be other position detectors capable of detecting the position of the motor, such as a resolver. The motor encoder 14 outputs encoder time series signals such as the angle, angular velocity, angular acceleration, and torque of the motor. When the machine 10 is a multi-axis machine such as a multi-joint robot or a multi-axis machine tool, the motor encoder 14 is provided on each axis of the machine 10. The sensor 13 and the motor encoder 14 are communicably connected to the control device 20 via wires or wirelessly. In addition, to prevent communication delays between the sensor 13 and the motor encoder 14, especially when using a wireless sensor, it is assumed that the time is synchronized between the wireless sensor and the control device 20.

The control device 20 includes a delay time calculation unit 21, a signal waveform correction unit 22, and a trajectory estimation unit 23. The delay time calculation unit 21 calculates the delay time from the motor encoder 14 to the machine tip unit 15 caused by at least one of the machine mechanism unit 11 and the tool 12 based on the degree of correlation between the sensor time series signal estimated or acquired from the sensor 13 and the encoder time series signal acquired or estimated from the motor encoder 14. The degree of correlation may be the multiplication mean, the sum of absolute differences (SAD), the sum of squared differences (SSD), normalized cross-correlation (NCC), or zero-mean normalized cross-correlation (ZNCC), between the sensor time series signal and the encoder time series signal. The delay time calculation unit 21 calculates the degree of correlation while shifting the encoder time series signal on the time axis relative to the sensor time series signal, or while shifting the sensor time series signal on the time axis relative to the encoder time series signal, and calculates the delay time at the point when the degree of correlation is maximum.

Figure 2A shows an example of a delay time caused by at least one of the machine mechanism 11 and the tool 12. Figure 2 shows the machine tip side where the sensor 13 is provided, the motor side where the motor encoder 14 is provided, and the machine mechanism 11 and the tool 12 between the machine tip side and the motor side. For example, when the motor accelerates, the encoder estimated position (shown by the dashed line) of the machine tip 15 estimated by the motor encoder 14 does not take into account the delay time caused by the deflection, backlash, etc. of at least one of the machine mechanism 11 and the tool 12, whereas the sensor estimated position (shown by the solid line) of the machine tip 15 estimated by the sensor 13 is the current position (actual position) including the delay time from the motor encoder 14 to the machine tip 15. Therefore, the delay time calculation unit 21 may calculate the delay time from the motor encoder 14 to the machine tip 15 using the sensor time series signal.

Figure 2B shows an example of the encoder estimated position (shown by a dashed line) corrected based on the delay time. Since the delay time changes from moment to moment depending on the stiffness, speed, acceleration, load, attitude, etc. of at least one of the mechanical mechanism unit 11 and the tool 12, it is advisable to correct the encoder estimated position from moment to moment based on the delay time. In other words, the signal waveform correction unit 22 corrects the waveform shape of the encoder time series signal based on the calculated delay time.

When the machine 10 is a multi-axis machine such as an articulated robot or a multi-axis machine tool, the waveform of the encoder time series signal may be corrected in the dimension of each axis of the machine 10, or in the dimension of the machine tip 15 (the dimension of the Cartesian coordinate system). When correcting in the dimension of each axis, the encoder time series signal may be time series data such as angle, angular velocity, angular acceleration, torque, etc. obtained from the motor encoder 14 of each axis, and the sensor time series signal may be time series data such as angle, angular velocity, angular acceleration, torque, etc. of each axis estimated by inverse kinematics from the position, speed, acceleration, load, etc. obtained from the sensor 13 of the machine tip 15. On the other hand, when correcting in the dimension of the machine tip 15, the encoder time series signal may be time series data such as position, speed, acceleration, load, etc. of the machine tip 15 estimated by forward transforming the angle, angular velocity, angular acceleration, torque, etc. obtained from the motor encoder 14 of each axis, and the sensor time series signal may be time series data such as position, speed, acceleration, load, etc. obtained from the sensor 13 of the machine tip 15.

Figure 3 shows an example of preprocessing of the sensor time series signal and the motor encoder time series signal. In this example, the explanation is based on an example of correction in the dimension of the machine tip 15. For example, the sensor time series signal is time series data of the acceleration of the machine tip 15, and the encoder time series signal is time series data of the position of the machine tip 15 estimated by forward transforming the angle acquired from the motor encoder 14 of each axis. The control device 20 may further include a preprocessing unit 28 that preprocesses the sensor time series signal and the motor encoder time series signal before calculating the delay time. For example, the preprocessing unit 28 includes a differentiator 30, a low-pass filter 31, and a high-pass filter 32.

The differentiator 30 differentiates the encoder time series signal (position) twice to convert it into time series data of the acceleration of the machine tip 15. This allows the components of the encoder time series signal (position) to match the components of the sensor time series signal (acceleration). The low-pass filter 31 is applied to both the sensor time series signal and the motor encoder time series signal, and passes only the low-frequency components used for trajectory estimation by the motor encoder 14. This makes it possible to calculate the delay time only in the required frequency band, and to match the frequency bands of the sensor time series signal and the motor encoder time series signal. The high-pass filter 32 is applied to both the sensor time series signal and the motor encoder time series signal, and removes the offset error of the sensor 13. The offset error is the output error of the sensor 13, and includes, for example, an error caused by calibration and an error caused by temperature. For example, in the case of an acceleration sensor, the offset error is a steady output error output at 0 G. The relationship between the cutoff frequency L1 of the low-pass filter 31 and the cutoff frequency H1 of the high-pass filter 32 may be L1>H1.

Figures 4A and 4B show an example of a waveform before the offset error of the sensor 13 is removed. As shown in Figure 4A, the sensor time series signal (shown by the thin line) is shifted in the signal magnitude direction from the encoder time series signal (shown by the thick line) by the offset error amount, so it is necessary to shift the sensor time series signal in the direction of the arrow by the offset error amount. As shown in Figure 4B, if the degree of correlation between the sensor time series signal and the encoder time series signal is calculated without removing the offset error, the degree of correlation may be maximum at the wrong time point, in which case a secondary error may occur.

Figures 5A and 5B show an example of a waveform after removing the offset error of the sensor 13. As shown in Figure 5B, when the degree of correlation between the sensor time series signal and the encoder time series signal is calculated after removing the offset error, the degree of correlation is maximized at the correct time point.

Referring again to FIG. 3, the delay time increases as the change in speed or the magnitude of acceleration increases, and decreases as the change in speed or the magnitude of acceleration decreases. Therefore, the delay time calculation unit 21 may calculate the delay time from moment to moment according to the change in speed or the magnitude of acceleration. Alternatively, the delay time calculation unit 21 may calculate the delay time for each predetermined interval according to the change in speed or the magnitude of the encoder time series signal. The signal waveform correction unit 22 corrects the encoder time series signal (position) based on the calculated delay time. Note that this encoder time series signal (position) is not the time series data of acceleration, but is the time series data of position, i.e., the estimated trajectory of the machine tip 15 by the motor encoder 14. The trajectory estimation unit 23 estimates the actual trajectory of the machine tip 15 based on the corrected encoder time series signal (position) and the sensor time series signal (position). This sensor time series signal (position) is not the time series data of acceleration, but is the time series data of position, i.e., the estimated trajectory of the machine tip 15 by the sensor 13.

Figure 6 shows an example of calculating the delay time without dividing the interval. The delay time calculation unit 21 may calculate the delay time at the point when the degree of correlation between the sensor time series signal and the encoder time series signal becomes maximum without dividing the interval.

Figure 7 shows an example of calculating the delay time for each section. If the encoder time series signal is time series data of acceleration or load, the sections should be divided according to the magnitude of acceleration or load (i.e., parallel to the time axis). On the other hand, if the encoder time series signal is time series data of speed, the sections should be divided according to the amount of change in speed (i.e., perpendicular to the time axis). The sections include sections where the magnitude or amount of change of the encoder time series signal is large and sections where it is small. There may be multiple thresholds defining such sections, for example, ±0.1, ±0.3, ±0.5, ... etc.

First, the delay time calculation unit 21 calculates the approximate delay time at which the degree of correlation between the sensor time series signal and the encoder time series signal is at its maximum in all sections. Then, the encoder time series signal is divided into sections, and the delay time at which the correlation is at its maximum in each section is calculated under the predetermined constraints that (1) it is within a predetermined time from the approximate delay time, and (2) the difference in delay time between adjacent sections is within a predetermined time. By calculating the delay time under certain constraints in this way, it is possible to eliminate the possibility of the degree of correlation being at its maximum at the wrong time, resulting in secondary errors.

The waveform of the encoder time series signal after division into sections has two patterns, a peak section and a slope section, so the delay time can be calculated as follows.
(1) In the case of a mountain section, the delay time at the peak is set to the delay time found in that section.
(2) In the case of a hill section, the delay time at the middle of the hill is the delay time found for that section.

The delay time calculation unit 21 may gradually switch the delay time between sections. For example, if the delay time of the slope section is (acc1, delay1) and the delay time of the mountain section is (acc2, delay2), the delay time (acc, delay) between the slope section and the mountain section may be calculated as delay = {(delay2-delay1)/(acc2-acc2)}*(acc-acc1)+delay1. By weighting and interpolating the delay time between sections in this way, it becomes possible to smoothly correct the waveform of the encoder time series signal. However, if the change in the delay time at each time is steep, the time series of the waveform of the encoder time series signal corrected by the delay time may be reversed. In that case, the delay time time series data may be smoothed using a low-pass filter or the like.

As another method of dividing the sections, as shown in FIG. 6, a section with a low degree of correlation may be divided at the point where the degree of correlation is maximized across the entire section, the waveform of the encoder time series signal may be modified in that section, and then a section with a low degree of correlation may be divided at the point where the degree of correlation is maximized across the entire section of the modified encoder time series signal waveform, and the waveform of the encoder time series signal may be modified in that section, and the process may be repeated to divide the sections. Also, instead of performing the weighted interpolation described above, another filter such as a moving average filter may be used to smoothly switch the delay time in each section.

Figure 8 shows an example of a trajectory estimation unit. The trajectory estimation unit 23 estimates the actual trajectory of the machine tip 15 based on the sensor time series signal (position) and the corrected encoder time series signal (position). This sensor time series signal (position) is an estimated trajectory of the machine tip 15 estimated from the speed, acceleration, load, etc. of the sensor 13, so estimation errors accumulate in the low-frequency components. Therefore, it is preferable for the trajectory estimation unit 23 to estimate the actual trajectory of the machine tip 15 based on the high-frequency components of the sensor time series signal (position) and the low-frequency components of the encoder time series signal (position).

For example, the trajectory estimation unit 23 may include a subtractor 40, a high-pass filter 41, and an adder 42. The subtractor 40 subtracts the encoder time series signal from the sensor time series signal. The high-pass filter 41 removes low-frequency components from the subtracted time series signal. This makes it possible to remove accumulated estimation errors. The adder 42 adds the encoder time series signal to the time series signal from which the low-frequency components have been removed. This allows an actual trajectory to be estimated based on the high-frequency components of the sensor time series signal (position) and the low-frequency components of the encoder time series signal (position).

Referring again to FIG. 1, the control device 20 may further include a memory unit 24, an operation control unit 25, a machine drive unit 26, and a tool drive unit 27. The memory unit 24 is, for example, a semiconductor memory, a magnetic memory, etc., and stores an operation program in advance. The operation control unit 25 is, for example, a processor, and generates an operation command based on a target trajectory included in the operation program and an estimated actual trajectory. The machine drive unit 26 is, for example, a motor drive circuit, and drives the mechanical mechanism unit 11 based on the generated operation command. The tool drive unit 27 is, for example, a tool drive circuit, and drives the tool 12 based on the generated operation command.

Referring again to FIG. 8, the above-mentioned operation control unit 25 may include a subtractor 50 and a low-pass filter 51. The subtractor 50 subtracts the target trajectory from the estimated actual trajectory. The low-pass filter 51 is not an essential component, but it removes noise and high-frequency components that are difficult to control. The relationship between the cutoff frequency H2 of the high-pass filter 41 and the cutoff frequency L2 of the low-pass filter 51 may be H2<L2. This generates an estimated deviation between the actual trajectory and the target trajectory. The operation control unit 25 may perform learning control (repetitive control) or PID (proportional, integral, derivative) control based on this estimated deviation.

Figure 9 shows a schematic configuration of a machine system 1 in another embodiment. The machine system 1 in this example is a server system or a cloud system, and includes a trajectory estimation device 60 arranged on a WAN (wide area network) or a LAN (local area network). The trajectory estimation device 60 is connected in a one-to-many manner, via wire or wireless, to a plurality of sensors 13 provided on each of the plurality of machines 10, a plurality of motor encoders 14 provided on each of the plurality of machines 10, and a plurality of control devices 20 that control the plurality of machines 10. The trajectory estimation device 60 includes a delay time calculation unit 21, a signal waveform correction unit 22, and a trajectory estimation unit 23.

The delay time calculation unit 21 receives multiple sensor time series signals and multiple encoder time series signals from the multiple sensors 13 and multiple motor encoders 14, and calculates the delay time caused by at least one of the machine mechanism unit 11 and the tool 12 based on the degree of correlation between the sensor time series signals and the motor encoder time series signals. The signal waveform correction unit 22 corrects the waveforms of the multiple encoder time series signals based on the calculated delay time. The trajectory estimation unit 23 estimates the actual trajectories of the multiple machine tips 15 based on the multiple sensor time series signals and the corrected multiple motor encoder time series signals, and transmits the estimated actual trajectories to the multiple control devices 20, respectively. The control devices 20 estimate the deviation between the received actual trajectories and the target trajectory included in the operation program, and control the vibration of each of the machine tips 15 by performing learning control or PID control based on the estimated deviation.

According to the above embodiment, the waveform of the encoder time series signal is corrected using the sensor time series signal including the delay time from the motor encoder 14 to the machine tip 15, so that an encoder time series signal that takes the delay time into account can be generated. As a result, the actual trajectory of the machine tip can be accurately estimated based on the sensor time series signal and the encoder time series signal.

The program applied to the above-mentioned processor may be provided by recording it on a computer-readable non-transitory recording medium, such as a CD-ROM, or may be provided by distributing it from a server device on a WAN (wide area network) or LAN (local area network) via wired or wireless communication.

Although various embodiments have been described herein, it should be recognized that the present invention is not limited to the above-described embodiments and that various modifications may be made within the scope of the following claims.

REFERENCE SIGNS LIST 1 Mechanical system 10 Machine 11 Mechanical mechanism 12 Tool 13 Sensor 14 Motor encoder 15 Machine tip 20 Control device 21 Delay time calculation section 22 Signal waveform correction section 23 Trajectory estimation section 24 Memory section 25 Operation control section 26 Machine drive section 27 Tool drive section 30 Differentiator 31 Low-pass filter 32 High-pass filter 40 Subtractor 41 High-pass filter 42 Adder 50 Subtractor 51 Low-pass filter

Claims (15)

A machine system that estimates a trajectory of a machine using a sensor provided at a machine tip and a motor encoder provided at a machine mechanism,
a delay time calculation unit that calculates a delay time from the motor encoder to the tip of the machine based on a degree of correlation between a sensor time series signal estimated or acquired by the sensor and an encoder time series signal acquired or estimated by the motor encoder;
a signal waveform correction unit that corrects a waveform of the encoder time-series signal based on the calculated delay time;
a trajectory estimation unit that estimates an actual trajectory of the tip of the machine by subtracting the corrected encoder time series signal from the sensor time series signal, removing low frequency components from the sensor time series signal from which the encoder time series signal has been subtracted, and adding the encoder time series signal to the sensor time series signal from which the low frequency components have been removed ;
A mechanical system comprising: The mechanical system of claim 1, wherein the degree of correlation is a multiplicative average, a sum of absolute differences, a sum of squared differences, a normalized cross-correlation, or a zero-mean normalized cross-correlation between the sensor time series signal and the encoder time series signal. The mechanical system according to claim 1 or 2, wherein the delay time calculation unit calculates the degree of correlation while shifting the encoder time series signal on the time axis relative to the sensor time series signal, or while shifting the sensor time series signal on the time axis relative to the encoder time series signal. The mechanical system according to any one of claims 1 to 3, further comprising a low-pass filter that passes only low-frequency components used for trajectory estimation by the motor encoder before calculating the delay time. The mechanical system according to any one of claims 1 to 4, further comprising a high-pass filter that removes an offset error of the sensor before calculating the delay time. The mechanical system according to any one of claims 1 to 5, wherein the delay time calculation unit calculates the delay time according to the amount or magnitude of change in the encoder time series signal. The mechanical system according to claim 6, wherein the delay time calculation unit calculates the delay time for each predetermined interval according to the amount or magnitude of change in the encoder time series signal. The mechanical system according to claim 7, wherein the section includes a section in which the amount or magnitude of change in the encoder time series signal is large and a section in which the amount or magnitude of change is small. The mechanical system according to claim 7 or 8, wherein there are multiple thresholds that define the interval. The mechanical system according to any one of claims 7 to 9, wherein the delay time calculation unit calculates the delay time at which the correlation is maximized in each section under a predetermined constraint. The mechanical system according to any one of claims 7 to 10, wherein the signal waveform correction unit gradually switches the delay time between the sections. The machine system according to any one of claims 1 to 11, wherein the machine is a multi-axis machine, and the signal waveform correction unit corrects the waveform of the encoder time series signal in the dimension of each axis of the machine or in the dimension of the machine tip. The mechanical system according to claim 12, wherein the encoder time series signal is time series data of the angle, angular velocity, angular acceleration, or torque of each of the axes, or time series data of the position, velocity, acceleration, or load of the tip of the machine. The mechanical system according to any one of claims 1 to 13, wherein the sensor includes an acceleration sensor, a gyro sensor, or an inertial sensor. The mechanical system according to any one of claims 1 to 14, wherein the trajectory estimation unit estimates the actual trajectory of the machine tip based on high-frequency components of the sensor time series signal and low-frequency components of the encoder time series signal.

Images