Patent ID: 9199374
Filing Date: 2015-12-01
Classification: B25J,G05B,Y10S

Abstract:
1. A control unit of an articulated robot for controlling the articulated robot, the articulated robot including a first articulated drive system having a working part that moves along a working line of a workpiece on a top end thereof and three drive shafts that are connected to the working part and that change the attitude of the working part and a second articulated drive system having at least three drive shafts that are connected to the first articulated drive system and that change the position of the first articulated drive system, the unit comprising: interpolation data calculating means for calculating data describing the position and attitude of the working part at each of a plurality of interpolated points that lie on a previous teaching path connecting the work start point and attitude to the work end point and attitude of the working part in a base coordinate system of the articulated robot; angle calculating means for calculating the angles of all of the drive shafts in the first articulated drive system and the second articulated drive system, the angles representing the position and the attitude of the working part at each of the interpolated points by finding a solution to an inverse kinematics problem from the data calculated by the interpolation data calculating means; speed calculating means for calculating the speeds of two drive shafts located at both ends of the first articulated drive system when the attitude of the working part is moved to the next interpolated point on the basis of a difference between the angle of each of the drive shafts of the first articulated drive system at a current interpolated point and the angle at the next interpolated point calculated by the angle calculating means; attitude data converting means for converting the data calculated by the interpolation data calculating means into attitude data describing the attitude of the working part in a work coordinate system having an axis extending in a moving direction of the working part, an axis extending in a direction that is perpendicular to the axis in the moving direction, and an axis perpendicular to each of the two axes; recalculating means for, if the speed of at least one of the two drive shafts located at both the ends of the first articulated drive system and calculated by the speed calculating means is outside a predetermined allowable range, recalculating the angles of the drive shafts of the first articulated drive system so that the speeds of the two drive shafts located at both the ends of the first articulated drive system are within the allowable range without changing the moving speed of the working part and with reducing variation of one or two of particular components of the attitude data of the working part at the next interpolated point converted by the attitude data converting means and recalculating the angles of the drive shafts of the second articulated drive system on the basis of the calculated angles of the drive shafts of the first articulated drive system and the position of the working part calculated by the interpolation data calculating means; the recalculating means includes: angle candidate calculating means for calculating, on the basis of the angle of each of the drive shafts of the first articulated drive system at the current interpolated point, a plurality of candidates of the angle of each of the drive shafts so that the angle of each of the drive shafts follows the angle at the next interpolated point calculated by the angle calculating means and, in addition, the speeds of two drive shafts located at both ends of the first articulated drive system at the next interpolated point are within the allowable range, attitude data calculating means for calculating attitude data that defines the attitude of the working part at the next interpolated point in the work coordinate system when each of the candidates of the angle calculated by the angle candidate calculating means is employed, and angle selecting means for selecting, from among the plurality of attitude data calculated by the attitude data calculating means, attitude data having the smallest variation of the particular component for the attitude data of the working part at the next interpolated point converted by the attitude data converting means and selecting the angle corresponding to the attitude data as the angle of the drive shaft; and drive instructing means for, if the speeds of the two drive shafts located at both ends of the first articulated drive system calculated by the speed calculating means are within the allowable range, driving the articulated robot on the basis of the angles of the drive shafts calculated by the angle calculating means and, if the speed of at least one of the two drive shafts located at both ends of the first articulated drive system calculated by the speed calculating means is outside the allowable range, driving the articulated robot on the basis of the angles of the drive shafts calculated by the recalculating means.