MOTION CONTROL METHOD AND DEVICE FOR ARTICULATED MANIPULAOR

PURPOSE:To achieve stable action at and around a singular point by using a pseudo inverse matrix at the singular point where a determinant of a Jacobian matrix is a specific one, and using a matrix of an inverse matrix combined with the pseudo inverse matrix at the singular point closest to its neighboring point at the neighboring point to the singular point, and using the inverse matrix at other points than these. CONSTITUTION:An approximate inverse Jacobian matrix computation part 1a'' of a speed control device 1'' composing an articulated manipulator motion control syste computes an approximate inverse matrix J# (theta) based on a current articulation angle theta. For this approximate inverse matrix J#(theta), a pseudo inverse matrix of a Jacobian matrix is used at a singular point where a determinant of the Jacobian matrix is zero. At a neighboring point to the singular point, a matrix of an inverse matrix synthesized with a pseudo inverse matrix at the closest singular point to the neighboring point is used. At the singular point and other points than the singular point, the inverse matrix of the Jacobian matrix is used. By thus using the determined approximate inverse matrix J#(theta), a speed command value vector Vc is multiplied to determine an articulation drive speed command b'', thereby an articulation angle speed vector thetac is outputted to a speed control part 1b''.