To avoid a singularity of a manipulator by slightly correcting the position of an auxiliary device.
When a singularity determining unit 26 determines the vicinity of a singularity, a correction moving device target value calculating unit 27 corrects an auxiliary device target value Jsta(t) to calculate a correction auxiliary device target value Jsta(t)' so that the posture of a manipulator at the next time becomes out of a range of the vicinity of the singularity while maintaining the position and posture of a tool front end to a work at the next time. A second coordinate converting/calculating unit 28 converts a tool target value worldPta(t) into a tool target value defined in a manipulator coordinate system base, using the corrected auxiliary device target value Jsta(t)'. A correction target joint angle calculating unit 29 carries out inverse kinematics calculation on the converted tool target value worldPta(t)" defined in the manipulator coordinate system base to calculate a correction target joint angle Jmta(t)". The correction target joint angle Jmta(t)" and the corrected auxiliary device target value Jsta(t)' are outputted to the manipulator 11 and the moving device 12.
| JP4852719 | Articulated robot |
| JPH06297360 | WORKING ROBOT AND OPERATION THEREOF |
| WO/2017/002143 | SURGICAL ROBOT |
Takuji Yamada
Atsushi Maeda
Yoshiyuki Maebori