Este documento presenta un método eficiente para asegurar el rendimiento de la navegación al suprimir el riesgo de divergencia del SLAM LiDAR a través de un análisis de observabilidad geométrica propuesto recientemente en un mapa de nubes de puntos tridimensional. Para ello, se introducen características de observabilidad que evalúan cuantitativamente la calidad de la distribución geométrica de las características. Específicamente, este estudio adapta una matriz de observabilidad geométrica 3D y el número de condición asociado para desarrollar un beneficio numérico. En una aplicación extensa, implementamos la planificación de rutas en la que el SLAM mejorado funciona sin problemas basado en el método propuesto. Finalmente, para validar el rendimiento del algoritmo propuesto, se realizó un estudio de simulación utilizando el simulador Gazebo de alta fidelidad, donde se demuestra la estrategia de planificación de rutas de un dron dependiendo de la calidad de navegación. Además, se presenta un resultado experimental de un vehículo autónomo en interiores para apoyar la efectividad del algoritmo propuesto.