Motion Planning and Control of Contact Task Based on Diffeomorphic Mapping Dynamical Systems

The existing time-dependent motion planning and control methods show a poor anti-disturbance ability when the robot performs contact tasks. This paper proposes a motion planning and control method based on a diffeomorphic mapping dynamical system to solve the problem. Firstly, the position and force data of human executing contact tasks are collected on the established experiment platform by kinesthetic teaching, and a dynamical system based on diffeomorphic mapping is developed. Then, the contact task with human disturbances is decomposed into three states: executing the contact task, moving under human disturbances, and moving in free space. In addition, adjustment terms for force and orientation control are designed for each state based on the dynamical system. Finally, experiments are conducted in which the robot performs the contact task with and without human disturbances. The results of force control based on the proposed method are compared with the results of the offline control method. In comparison with the demonstration trajectory, the maximum position and orientation errors in the 10 repeated experiments without human disturbances are less than 0.008 m and 1.01°, respectively. In addition, the average value of the root mean square error (RMSE) of force is 0.86 N, which is slightly larger than the offline control method. The robot can reach the contact space to perform the contact tasks from any position in free space. When a disturbance occurs during the operation process, the robot will follow the instructions of the operator and resume the tasks after the disturbance with a force RMSE similar to the result without human disturbances. The experimental results confirm the effectiveness of the proposed approach, which realizes motion planning and control for performing contact tasks under human disturbances while ensuring a good anti-disturbance ability.