Sensor device, object sensing method, and robot system

Abstract

This sensor device is provided with: a detection unit that carries out at a predetermined periodicity a detection process for detecting an object; and a position estimation unit that, carrying out a filtering process on the basis of a present-instance surveyed position detected by the detection unit in a present-instance detection process, and a surveyed position detected in the past, estimates an estimated position of the object on a per-predetermined-period basis, and if the present-instance surveyed position is closer to the sensor device than the estimated position estimated in the filtering process, uses the present-instance surveyed position as the estimated position of the object in place of the estimated position estimated in the filtering process.

Description

Sensor device, object detection method, and robot system

[0001] This specification discloses a sensor device, an object detection method, and a robot system.

[0002] Conventionally, a radar device has been proposed that includes a detection unit that performs a detection process to detect targets at a predetermined cycle, a determination unit that determines whether the target detected in the current process is the same target that has temporal continuity with the target detected in the previous process, and a filter processing unit that performs filter processing (processing using a Kalman filter or a particle filter) that averages multiple instantaneous values ​​that are processed in a time series for targets that have been determined to be the same target (see, for example, Patent Document 1).

[0003] Japanese Patent Application Laid-Open No. 2018-124209

[0004] Because filtering averages multiple instantaneous values ​​(time-series data), it takes some time for the observed position of an object to converge, which can cause a time delay in detecting an object that newly enters the detection area of ​​the sensor device.

[0005] A main object of the present disclosure is to suppress the occurrence of a time delay in detecting an object that enters a detection area, thereby further improving safety.

[0006] The present disclosure has adopted the following means to achieve the above-mentioned main object.

[0007] The sensor device of the present disclosure is a sensor device that detects an object, and includes: a detection unit that performs a detection process to detect an object at a predetermined cycle; and a position estimation unit that performs a filter process based on a current observation position detected by the detection unit in the current detection process and previously detected observation positions to estimate an estimated position of the object at each predetermined cycle, and when the current observation position is closer to the sensor device than the estimated position estimated by the filter process, sets the current observation position as the estimated position of the object instead of the estimated position estimated by the filter process.

[0008] In the sensor device disclosed herein, a detection unit performs filtering based on the current observation position detected in the current detection process and previously detected observation positions to estimate the estimated position of an object at predetermined intervals. This allows the object's position to be estimated with high accuracy even if there is variation in the observation position of the object detected by the detection unit. On the other hand, if the current observation position is closer to the sensor device than the estimated position estimated by filtering, the current observation position is used as the estimated position of the object instead of the estimated position estimated by filtering. Filtering effectively reduces time delays in detecting objects closer to the sensor device, further improving safety.

[0009] The object detection method of the present disclosure is an object detection method that detects an object, comprising: a detection step of performing a detection process to detect an object at a predetermined cycle; and a position estimation step of performing a filter process based on a current observation position detected in the detection process at the present time and previously detected observation positions to estimate an estimated position of the object at the predetermined cycle, and if the current observation position is closer to the sensor device than the estimated position estimated by the filter process, setting the current observation position as the estimated position of the object instead of the estimated position estimated by the filter process.

[0010] The object detection method of the present disclosure can achieve the same effects as the sensor device of the present disclosure.

[0011] The robot system of the present disclosure is summarized as comprising: a robot; a sensor device including a detection unit that performs a detection process to detect an object at a predetermined cycle; and a position estimation unit that performs a filter process based on a current observation position detected by the detection unit in the current detection process and previously detected observation positions to estimate an estimated position of the object at a predetermined cycle, and if the current observation position is closer to the sensor device than the estimated position estimated by the filter process, sets the current observation position as the estimated position of the object instead of the estimated position estimated by the filter process; and a robot control unit that controls the robot based on the estimated position.

[0012] The robot system of the present disclosure includes the sensor device of the present disclosure, and therefore can suppress time delays in detecting an object that newly enters the detection area. Therefore, by controlling the robot based on the estimated position estimated by the sensor device, safety during operation of the robot can be further improved.

[0013] 1 is a schematic configuration diagram of a robot system according to the present disclosure; FIG. 2 is a block diagram showing the electrical connection relationship between a robot main body, a robot control device, and a sensor device; FIG. 3 is a flowchart showing an example of an object detection process executed by a control unit of the sensor device; FIG. 4 is an explanatory diagram showing the relationship between the distance from the sensor device to an object and the speed limit value of the robot; FIG. 5 is an explanatory diagram showing the previous observation position, an object existence region, and a current observation position; FIG. 6 is an explanatory diagram showing the current object existence region (current estimated position) calculated by position estimation using a Kalman filter; and FIG. 7 is an explanatory diagram showing the current estimated position set from the current observation position. FIG. 8 is a schematic configuration diagram of a robot system according to another embodiment.

[0014] Next, embodiments of the present disclosure will be described with reference to the drawings.

[0015] Fig. 1 is a schematic configuration diagram of a robot system 1 according to the present disclosure. Fig. 2 is a block diagram showing the electrical connection relationship between a robot main body 10, a robot control device 20, and a sensor device 30. As shown in the figure, the robot system 1 according to this embodiment includes the robot main body 10, the robot control device 20 that controls the operation of the robot main body 10, and a sensor device 30 that can detect interfering objects around the robot main body 10.

[0016] The robot body 10 is a work robot that performs a specified task, and examples thereof include a processing robot that processes a workpiece with a tool, a transport robot that grasps a workpiece with a chuck and transports it to another position, and an assembly robot that grasps a workpiece with a chuck and assembles it to an object.

[0017] The robot main body 10 has a base 11 and an articulated arm 12 mounted on the base 11. The articulated arm 12 has a plurality of arms connected in series to the base 11 via joint shafts. Each joint shaft is provided with a servo motor 15 that drives the corresponding joint shaft, and an encoder 16 (rotary encoder) that detects the rotation angle of the corresponding servo motor 15. The robot main body 10 also has an amplifier unit 17 that applies a driving current to each servo motor 15.

[0018] The robot control device 20 includes a control unit 21 configured as a microprocessor including a CPU, ROM, and RAM, and an I / O port 22 for exchanging signals with the control unit 31 of the sensor device 30. The robot control device 20 also outputs control signals to the amplifier unit 17 of the robot main body 10 and inputs detection signals from the encoder 16.

[0019] The control unit 21 of the robot control device 20 controls the operation of the robot main body 10 as follows. Specifically, the control unit 21 first sets a target angle for each joint axis of the articulated arm 12 using inverse kinematics based on the target position and target posture of the hand. Next, the control unit 21 acquires the current angle of each joint axis from the corresponding encoder 16 and sets a speed command value for each joint axis using feedback calculations (e.g., proportional-integral calculations or proportional-integral-derivative calculations) based on the difference between the target angle and the current angle. Next, the control unit 21 multiplies the speed command value by a speed limit value (described later) that is set within a range from 0 (0%) to 1 (100%) to set a target speed that limits the speed command value. For example, if the speed limit value is 1 (100%), the target speed is the same as the speed command value. In other words, the robot speed is not limited. Furthermore, if the speed limit value is 0.5 (50%), the target speed is half the speed command value. Furthermore, if the speed limit value is 0 (0%), the target speed will be 0 regardless of the speed command value. In other words, the robot body 10 stops moving. Next, the control unit 21 calculates the current speed from the current angle of the joint axis obtained from the encoder 16, and sets a torque command value to be output from the servo motor 15 by feedback calculation (for example, proportional-integral calculation or proportional-integral-derivative calculation) based on the difference between the calculated current speed and the target speed. Then, the control unit 21 outputs a control signal to the corresponding amplifier unit 17 so that the servo motor 15 outputs a torque corresponding to the set torque command.

[0020] In this embodiment, the sensor device 30 is attached to the hand of the articulated arm 12. The sensor device 30 includes a control unit 31 configured as a microprocessor including a CPU, ROM, and RAM, a sensor unit 32 that monitors the surroundings, and an I / O port 33 for exchanging signals with the control unit 21.

[0021] In this embodiment, the sensor unit 32 is configured as a frequency modulation continuous wave (FMCW) radar sensor. The sensor unit 32 includes a transmitting antenna that transmits a transmitted chirp, a receiving antenna that receives a reflected chirp from an object, a mixer that mixes the transmitted chirp and the received chirp to generate an intermediate frequency signal (beat signal), and a processing unit that processes the beat signal to detect the object. The transmitting antenna transmits multiple transmitted chirps as one frame, each of which is modulated so that the frequency changes over time and spaced at regular intervals. The processing unit includes an A / D converter that performs A / D conversion on the beat signal generated by the mixer and a DSP that performs Fourier transform processing (FFT processing) on ​​the A / D converted beat signal. The DSP performs FFT processing (distance FFT processing) on ​​the beat signal on a chirp-by-chirp basis to obtain a distance spectrum indicating the signal strength for each frequency. By detecting frequencies (peak frequencies) with high signal strength, the distance at which the signal strength increases due to reflection from the object, i.e., the distance to the object, can be detected. Furthermore, the DSP performs FFT processing (velocity FFT processing) on ​​the data after the distance FFT processing on a frame-by-frame basis to obtain a velocity spectrum. Repeating the operation of obtaining a distance spectrum over an extremely short period of time results in the distance traveled by the object appearing as a phase difference in the distance spectrum. Therefore, by performing a Fourier transform using multiple distance spectra, a velocity spectrum corresponding to the distance traveled over an extremely short period of time, i.e., the velocity spectrum corresponding to the velocity, can be obtained. Object detection is performed by detecting points of high signal strength (peaks) from two-dimensional spectra corresponding to the distance and velocity obtained by the distance FFT processing and velocity FFT processing every predetermined period (one frame). The sensor unit 32 may be configured as a MIMO (multi-input, multi-output) radar sensor having multiple transmitting and receiving antennas. In this case, the DSP may perform FFT processing (angle FFT processing) on ​​the data after the velocity FFT processing on a receiving antenna-by-receiving antenna basis to obtain a frequency spectrum, and then detect the angle of the object relative to the sensor unit 32 based on the peaks.Here, the distance of an object detected by the sensor unit 32 or the combination of the distance and angle of an object detected by the sensor unit 32 is referred to as an observation position.

[0022] Next, a description will be given of the operation of the sensor device 30 of this embodiment. Fig. 3 is a flowchart showing an example of an object detection process executed by the control unit 31 (CPU) of the sensor device 30. This process is repeatedly executed at predetermined intervals.

[0023] In the object detection process, the control unit 31 first acquires the position (current observation position) and velocity (relative velocity) of the object detected currently from the sensor unit 32 (S100). Next, the control unit 31 calculates a predicted position (pre-estimated value) of the object through a prediction step of a Kalman filter based on the object's previously observed positions and velocities (S102). The control unit 31 then calculates an estimated position (posterior estimate) of the object through a correction step of the Kalman filter based on the current observation position and the predicted position (S104), and determines whether the current observation position is within an object existence region including the estimated position (S106). Here, the object existence region is an equal probability ellipse (a region in which an object may exist with a certain probability) with an equal Mahalanobis distance, and is used as a threshold for outlier detection based on the Mahalanobis distance. The control unit 31 calculates the Mahalanobis distance by determining the covariance between the current observation position and the predicted position calculated in S102, and then calculates the percentage probability of the calculated Mahalanobis distance occurring in a chi-square distribution. If the calculated probability is equal to or greater than a certain probability, the control unit 31 determines that the current observation position is within the object existence region (normal value). On the other hand, if the calculated probability is less than the certain probability, the control unit 31 determines that the current observation position is not within the object existence region (outlier value).

[0024] If the control unit 31 determines that the current observation position is within the object presence area, it sets the robot's speed limit value based on the estimated position calculated in S104 as the current estimated position (S108). The control unit 31 then transmits the speed limit value to the robot control device 20 via the I / O port 33 (S110) and terminates the object detection process. As shown in FIG. 4 , the speed limit value is set within a range of 0 (0%) to 1 (100%) so that the closer the current estimated position is to the sensor unit 32 (robot main body 10), the smaller the speed limit value (so that the speed of the robot main body 10 is greatly limited). By calculating the estimated object position based on the current observation position and the previous observation positions using a Kalman filter, the object position (current estimated position) can be accurately estimated even if there is variation in the observation position from the sensor unit 32. Setting the speed limit value based on the current estimated position can prevent collisions with objects (interfering objects) during operation of the robot main body 10, thereby improving safety.

[0025] In this embodiment, the control unit 31 sets the speed limit value of the robot main body 10 based on the current estimated position of the object detected by the sensor unit 32. However, the speed limit value may be set based on a combination of the current estimated position of the object and its speed (relative speed) detected by the sensor unit 32. In this case, the speed limit value may be set so that the speed of the robot main body 10 is more greatly limited as the time it takes for the object to reach the sensor unit 32 based on the current estimated value and speed becomes shorter.

[0026] If the control unit 31 determines in S106 that the current observation position is not within the object presence area, it determines whether the current observation position is closer to the sensor unit 32 than the estimated position calculated in S104 (S112). If the control unit 31 determines that the current observation position is not closer to the sensor unit 32 than the estimated position, it sets the predicted position calculated in S102 as the current estimated position (S114) and proceeds to S108. That is, the control unit 31 sets the current estimated position by excluding the current observation position as a mere outlier.

[0027] On the other hand, if the control unit 31 determines in S112 that the current observed position is closer to the sensor unit 32 than the estimated position calculated in S104, it sets the current observed position as the current estimated position (S116), clears information on past observed positions and their velocities used in the filter processing (Kalman filter prediction step) of S102 in the next control cycle (S118), and proceeds to S108. Therefore, in the next control cycle, the control unit 31 calculates the predicted position of the object in the prediction step based on the current observed position and the next observed position and their velocities obtained in S100 of the next control cycle. This makes it possible, when a new object is detected, to accurately estimate the position of the new object even if there is variation in the observed position of the detected object.

[0028] Consider a situation where a first object is detected within the monitoring area of ​​the sensor device 30, and a second object enters the vicinity of the sensor device 30 within the monitoring area, resulting in the current observation position, as shown in FIG. 5 . The estimated position (object presence area) calculated by the Kalman filter in S104 of the object detection process takes into account the previous observation positions, resulting in a time delay before converging on the observed position of the second object (see FIG. 6 ). Therefore, the current estimated position is farther from the sensor device 30 than the actual position of the second object. However, in this case, even though an object is present near the sensor device 30, the robot speed may not be sufficiently limited. In contrast, in this embodiment, if the current observation position occurs closer to the sensor device 30 than the estimated position (object presence area) calculated in S104, the control unit 31 sets the current observation position as the current estimated position (see FIG. 7 ). This allows the robot speed to be quickly and sufficiently limited in response to the second object entering the vicinity of the sensor device 30 within the monitoring area, thereby further improving safety.

[0029] Here, the correspondence between the main elements of the embodiment and the main elements of the present disclosure described in the claims will be described. That is, the sensor unit 32 of the present embodiment corresponds to the detection unit of the present disclosure, and the control unit 31 that executes the object detection process corresponds to the position estimation unit. Also, the robot main body 10 corresponds to the robot main body, the sensor device 30 corresponds to the sensor device, and the control unit 21 of the robot control device 20 corresponds to the robot control unit.

[0030] It goes without saying that the present disclosure is not limited to the above-described embodiments, and can be embodied in various forms as long as they fall within the technical scope of the present disclosure.

[0031] For example, in the above-described embodiment, if the current observation position is not within the object presence area and is closer to the sensor device 30 than the estimated position estimated in S104, the control unit 31 sets the current observation position as the current estimated position. However, if the current observation position is farther away from the sensor device 30 than a certain distance (for example, 1500 mm or more or 2000 mm or more in FIG. 4), the control unit 31 may exclude the current observation position as a simple outlier because there is no risk to safety. For example, the control unit 31 may set the predicted position calculated in S102 as the current estimated position.

[0032] In the above-described embodiment, the sensor device 30 is attached to the hand (tip) of the articulated arm 12 of the robot main body 10. However, the sensor device 30 may be attached to the base 11 of the robot main body 10. In addition, the sensor device 30 is attached to a stationary robot, but it may also be attached to a self-propelled robot. The self-propelled robot may be an arm robot with the above-described articulated arm, or a transport robot. For example, as shown in FIG. 8 , in a mounting line 100 having feeder tables arranged in the transport direction of a board and each having a detachable feeder 102, and including multiple component mounters 101 that pick up components from the feeders 102 and mount them on the board, the sensor device 30 may be an automatic exchange robot 110 that moves along the mounting line 100 to exchange the feeders 102 for each component mounter 101. The sensor device 30 is attached to the automatic exchange robot 110, and when an interfering object is detected in the monitoring area, the sensor device 30 outputs a speed limit value to the automatic exchange robot 110 according to the distance from the interfering object and the speed of the interfering object. The automatic exchange robot 110 travels at a limited travel speed by multiplying the input speed limit value by the speed command value.

[0033] As described above, the sensor device of the present disclosure estimates the estimated position of an object at predetermined intervals by performing filtering based on the current observation position detected by the detection unit in the current detection process and previously detected observation positions. This allows the object's position to be estimated with high accuracy even if there is variation in the observation position of the object detected by the detection unit. On the other hand, if the current observation position is closer to the sensor device than the estimated position estimated by filtering, the current observation position is used as the estimated position of the object instead of the estimated position estimated by filtering. Filtering effectively reduces time delays in detecting objects closer to the sensor device, further improving safety.

[0034] In the sensor device of the present disclosure, the position estimation unit may set an object presence area including the estimated position as the filtering process, and if the current observation position is not within the object presence area and is closer to the sensor device than the estimated position, the current observation position may be used as the estimated position of the object. This makes it possible to estimate the position of the object with higher accuracy and quickly respond to the detection of an object closer to the sensor device.

[0035] In the sensor device according to the present disclosure, the position estimation unit may set the current observed position as the estimated position of the object, and then perform the filtering process based on the current observed position and the observed position detected in the detection process of the next cycle to resume estimating the estimated position of the object. In this way, the position of a newly detected object can be estimated with high accuracy.

[0036] Furthermore, the present disclosure is not limited to the form of a sensor device, but may also be in the form of an object detection method or a robot system including a robot main body, a control device, and a sensor device. In the case of a robot system, the robot main body may be an arm robot including an arm. Furthermore, the robot main body may be an automatic exchange robot that moves along a predetermined traveling line and exchanges feeders for each component mounter in a mounting line that is lined up in the conveying direction of the board and has multiple component mounters that can each pick up components from a feeder and mount them on the board.

[0037] The present disclosure is applicable to industries such as the manufacturing of robots and sensor devices.

[0038] 1 Robot system, 10 Robot body, 11 Base, 12 Articulated arm, 15 Servo motor, 16 Encoder, 17 Amplifier unit, 20 Robot control device, 21 Control unit, 22 I / O port, 30 Sensor device, 31 Control unit, 32 Sensor unit, 33 I / O port, 100 Mounting line, 101 Component mounter, 102 Feeder, 110 Automatic exchange robot.

Claims

1. A sensor device for detecting an object, comprising: a detection unit that performs a detection process for detecting the object at a predetermined period; and a position estimation unit that performs a filtering process based on the current observation position detected by the detection unit in the current detection process and the observation positions detected in the past, estimates the estimated position of the object for each predetermined period, and sets the current observation position as the estimated position of the object instead of the estimated position estimated by the filtering process when the current observation position is closer to the sensor device than the estimated position estimated by the filtering process.

2. The sensor device according to claim 1, wherein the position estimation unit sets an object existence region including the estimated position as the filtering process, and sets the current observation position as the estimated position of the object when the current observation position is not within the object existence region and is closer to the sensor device than the estimated position.

3. The sensor device according to claim 1 or 2, wherein the position estimation unit resumes the estimation of the estimated position of the object by performing the filtering process based on the current observation position set as the estimated position of the object and the observation position detected in the detection process of the next period.

4. An object detection method for detecting an object, comprising: a detection step of performing a detection process for detecting the object at a predetermined period; and a position estimation step of performing a filtering process based on the current observation position detected in the current detection process by the detection step and the observation positions detected in the past, estimating the estimated position of the object for each predetermined period, and setting the current observation position as the estimated position of the object instead of the estimated position estimated by the filtering process when the current observation position is closer to the sensor device than the estimated position estimated by the filtering process.

5. A sensor device including a robot, a detection unit that performs a detection process for detecting an object at a predetermined cycle, and a position estimation unit that performs a filtering process based on the current observation position detected in the current detection process by the detection unit and the observation position detected in the past, and estimates the estimated position of the object every predetermined cycle. When the current observation position is closer to the sensor device than the estimated position estimated by the filtering process, the current observation position is used as the estimated position of the object instead of the estimated position estimated by the filtering process, and a robot control unit that controls the robot based on the estimated position. A robot system comprising:

Images