| 摘要 |
PURPOSE:To measure the angle of rotation of each link with high accuracy by inputting a signal outputted every time each link rotates by a certain angle and an angular velocity of rotation of each link, and processing them. CONSTITUTION:A link 3 is provided with a gyro package 15 and links 1-3 are provided with reference rotational angle detecting means 13-15 which output signal on very contant-angle rotation. Signals from them are inputted to a controller 20. Angular velocities omegax-omegaz from the package 15 are detected 22 and a for coordinate conversion from the link 3 to the link 0. Then, an angular velocity detection part 22, a coordinate conversion matrix arithmetich part 26, and a rotational angle arithmetic part 28 are used to obtain angles psi, theta and phi of rotation. Then, an arithmetic part 24 for a manipulated variable U0, a manipulated variable coordinate conversion part 25, and a coordinate conversion matrix normalization correcting arithmetic part 27 are used to cor rect an error in the matrix C0<3>. Then, a reference rotational angle detection part 21 and an error estimation arithmetic part 23 are used to estimate a rota tional angle error vector epsilon0 and a rotational angular velocity vector deltaomegai3<0>. Thus, the angle of rotation of each link is known accurately.
|
