调试了好久，终于过了，ORZ。
第一个问题是遇到障碍物，机器人就不能往下走了，但是遇到重复的点，机器人还是可以往下走，因此需要分开处理。
第二个问题是边界不能走。。。。
其实上面两个问题，都有人提到过，我再强调一遍吧。。