[Optimization] Enhancing D* Lite performance using heapq and lazy deletion

Hi,

The current implementation of the D* Lite algorithm in `PathPlanning/DStarLite` uses a standard Python list for the priority queue `U`, which is sorted using `.sort()` during every `update_vertex` and `compute_shortest_path` call. This leads to a time complexity of approximately $O(n \log n)$ for queue operations in each iteration.

I have developed an optimized version that utilizes a **Min-Heap** (`heapq`) and an **Entry Finder** dictionary to implement **Lazy Deletion**. This reduces the complexity of priority queue updates to $O(\log n)$ and lookups to $O(1)$.

### Reason for Change
1. **Scalability:** The current sorting-based approach becomes significantly slow as the grid size increases or when the map has high-frequency obstacle updates.
2. **Efficiency:** Using `heapq` with a dictionary for node tracking is the standard, high-performance way to implement incremental search algorithms.

### Proposed Changes
* Replace the `list.sort()` mechanism with `heapq`.
* Introduce an `entry_finder` to handle node priority updates efficiently.
* Maintain consistency in the animation logic with the existing implementation.

I have already implemented and tested these changes locally and would like to submit a Pull Request.

Beginne in PathPlanning/DStarLite und untersuche, wie die Liste U während update_vertex und compute_shortest_path sortiert wird. Vergleiche das bestehende Warteschlangenverhalten mit Pythons heapq und dem vorgeschlagenen Ansatz der verzögerten Löschung mit entry_finder, einschließlich der Animationslogik. Als erledigt gilt, wenn Aktualisierungen der Warteschlange wiederholtes Sortieren der Liste vermeiden und dabei das bestehende D* Lite-Verhalten und die Animation bewahren.

Vom Indexierungsmodell aus dem Issue-Text verfasst.