Das Kriterium für die Anti-dead-lock-routine muss ja eigentlich ein Kriterium folgender Art sein: " Wie oft pro Zeit erkennt der Roboter eine Kollision ?".
Das "wie oft pro Zeit" scheint mir in Deinem Algrotihmus zu fehlen.