El error de posición en un sistema de navegación inercial (INS) para vehículos autónomos submarinos (AUV) aumenta con el tiempo. La navegación asistida por el terreno puede ayudar a corregir estos errores de posición del INS. Para mejorar la precisión de coincidencia bajo grandes errores de posición inicial, se propone un algoritmo de coincidencia de terreno mejorado que comprende coincidencia de contornos del terreno (TERCOM), optimización por enjambre de partículas (PSO) y punto de contorno más cercano iterativo (ICCP), denominado TERCOM-PSO-ICCP. Inicialmente, se utiliza un TERCOM mejorado con un ángulo de rotación aumentado para minimizar los errores de rumbo y reducir el error de posición inicial. El enfoque de extremo de similitud evalúa los resultados de coincidencia iniciales, lo que lleva a una mayor precisión en los resultados iniciales. A continuación, se emplea PSO optimizado por colonia de abejas (ABC) para la coincidencia secundaria con el fin de reducir aún más el error de posición inicial y estrechar el área de coincidencia. Finalmente, se aplica el ICCP, utilizando la distancia de Mahalanobis como función objetivo, para la tercera coincidencia, aprovechando las capacidades de búsqueda fina del ICCP. La combinación efectiva de estos tres algoritmos mejora significativamente el efecto de coincidencia de la navegación asistida por el terreno. Dos pruebas muestran que el TERCOM-PSO-ICCP mejorado reduce efectivamente el error de coincidencia y corrige la posición del INS.