Robot SLAM Algorithm Based on Self-Detected Waypoint