Obstacle Avoidance Path Planning For Worm-Like Robot