Intelligent obstacle avoidance algorithm with hierarchical structure