摘要 |
PURPOSE:To smoothly pass by a singular point without deviating from a given locus by fixing the joint axis at the foot of the two prescribed joint axes as a speed right before a singular point is detected and controlling the position and the attitude by the other joint when a singular point is detected. CONSTITUTION:Data on the position and attitude X, Y, Z, TX, TY and TX after a minute time are set, and the fifth joint of COStheta5 is calculated. When the theta5 satisfies theta5>M (M is a constant to limit the range of singular points), it is considered that the singular point is detected. The speed theta4 of the fourth joint theta4 right before the singular point is detected is stored, and other thetak is calculated based on the speed thetak (k=1-6) of the fourth joint theta4, and the set position and attitude data X, Y, Z, TX, TY and TZ, thereby controlling joints theta1-theta6. Then fine changes in the attitudes TX, TY and TZ are calculated, and used as a reference for the next setting of attitude data. Thus the handle 11 of a robot smoothly moves, following the given locus.
|