协作机器人控制算法