此算法的优化效率和鲁棒形都非常好,稍加修改可用于机器人(The optimization of the algorithm is efficient and robust shape is very good, a little modification can be used for the robot)