Acoustic communication and imaging sonar guided AUV docking: system infrastructure, docking methodology and lake trials