Method of controlling position and attitude of working robot and its manipulator and apparatus thereof

Claim 1

A control system for controlling the position and attitude of a manipulator which is mounted on a moving body comprising an autonomously-traveling truck defining a moving coordinate system which is fixed with respect to said autonomously-traveling truck, said manipulator including a plurality of knuckles and a mechanical hand and which conducts various operations, comprising: a position and attitude error operation unit for receiving first instruction values representing the desired position and attitude of the hand, for receiving values representing the actual position and attitude of the hand, and for outputting error values representing the disparity between said desired position and said actual position; an instruction value operation unit, coupled to said position and attitude error operation unit, for receiving said error values, for receiving said values representing the actual position of the hand, and for outputting second instruction values representing a new position and attitude; a first coordinate transformation unit, coupled to said instruction value operation unit, for receiving said second instruction values, for receiving a first matrix which is the inverse of a second matrix representing the position and orientation of said truck, and for outputting a third matrix representing position and attitude instruction values based on said moving coordinate system; a backward kinematics operation unit, coupled to said first coordinate transformation unit, for receiving said third matrix and outputting a first set of knuckle angle vectors; a servo-system, coupled to said backward kinematics operation unit, for receiving said first set of knuckle angle vectors and outputting a second set of knuckle angle vectors representing the operating state of respective knuckles; a forward kinematics operation unit, coupled to said servo-system, for receiving said second set of knuckle angle vectors and outputting a fourth matrix representing position and attitude of said manipulator; a second coordinate transformation unit, coupled to said forward kinematics operation unit, for receiving said fourth matrix and said second matrix and outputting said values representing the actual position and attitude of the hand; and a position and orientation operation unit, coupled to said first and second coordinate transformation units, for receiving initial position and orientation estimated values of said truck and truck driving information and outputting said first and second matrices.