本研究旨在建立一套適用於小型自主式水下載具（Autonomous Underwater Vehicle, AUV）之前視聲納環境建圖與避障決策系統。由於GNSS訊號難以在水下環境中有效傳播，載具在實際航行過程中亦可能遭遇未知障礙物，若僅依照預設航點移動，將難以確保任務安全性。為提升小型AUV在水下環境中的自主航行能力，本研究以Raspberry Pi 5作為上層運算平台，Pixhawk 6C負責底層控制，並整合都卜勒測速儀（Doppler Velocity Log, DVL）、慣性量測單元（Inertial Measurement Unit, IMU）、壓力感測器與前視聲納，整體系統建構於機器人作業系統2（Robot Operating System 2, ROS 2）架構中，完成感測器資料接收、航位推算、導引控制、聲納資料處理、障礙物地圖建立等功能模組。
導航與控制部分，本研究利用DVL量測之速度及航向資料進行航位推算（Dead Reckoning, DR），系統會根據載具目前位置與目標航點之相對方位計算目標航向，再由增益調變PID控制器（Gain-Scheduled PID Controller, GS-PID）完成航向及離底高度控制。前視聲納資料處理與環境建圖部分，本研究建立聲納通訊與資料解析流程，完成RS-232封包讀取、參數設定、掃描線資料解析及回波強度處理。聲納回波經方位與距離解析後，結合載具位置與航向資料進行座標轉換及動態掃描補償，並經多時刻累積後建立離線二維障礙物地圖，另結合聲納下傾安裝幾何、底部邊界與垂向基準，建立局部三維地形高度表示，用以描述障礙物相對底部的高度變化。水平避障模擬部分採用人工勢場法（Artificial Potential Field, APF），將二維障礙物地圖中的障礙物資訊作為斥力來源，系統會依據吸引力與斥力的合成方向計算航向修正量。模擬結果顯示，載具軌跡可依障礙物分布調整航行方向，並避開障礙物邊界及其安全範圍。
控制實驗顯示，系統已具備基本航向修正與離底高度控制能力。聲納實驗則驗證牆面回波萃取、動態掃描補償、二維障礙物地圖建立及局部三維地形高度估測流程。實驗結果顯示，聲納掃描資料經回波篩選、座標轉換與多時刻累積後，可離線形成牆面邊界、二維障礙物地圖及局部地形高度表示。研究成果可作為實場閉迴路避障系統建置之基礎。

This study aims to develop a forward-looking sonar-based environmental mapping and obstacle avoidance decision-making system for a small autonomous underwater vehicle (AUV). Since Global Navigation Satellite System (GNSS) signals cannot be effectively transmitted underwater, the vehicle must rely on onboard sensors for navigation and localization. To improve the autonomous navigation capability of a small AUV, this study integrates a Raspberry Pi 5, Pixhawk 6C, Doppler Velocity Log (DVL), Inertial Measurement Unit (IMU), pressure sensor, and forward-looking sonar within the Robot Operating System 2 (ROS 2) framework. The system includes sensor data acquisition, dead reckoning, guidance control, sonar data processing, and obstacle map generation.
For navigation and control, DVL velocity and heading data are used for dead reckoning (DR) to estimate the vehicle position. The target heading is calculated according to the relative bearing between the current vehicle position and the target waypoint. Heading tracking is performed using a gain-scheduled proportional-integral-derivative controller (GS-PID), while altitude-above-bottom control is achieved using DVL altitude feedback. For sonar mapping, the Micron DST Sonar scanline data are decoded through RS-232 communication and processed through echo intensity filtering, coordinate transformation, and dynamic scan compensation. The compensated sonar data are accumulated to construct an offline two-dimensional obstacle map. In addition, a local three-dimensional terrain height representation is established based on the sonar mounting geometry, bottom-boundary information, and a vertical reference.
The artificial potential field (APF) method is adopted for horizontal obstacle avoidance simulation. The two-dimensional obstacle map is used as the repulsive source, and the target waypoint is used as the attractive direction to calculate the heading correction. The control experiments show that the system has basic heading correction and altitude-above-bottom control capabilities. The sonar experiments verify wall echo extraction, dynamic scan compensation, two-dimensional obstacle map generation, and local terrain height estimation. These results provide a foundation for future implementation of a field closed-loop obstacle avoidance system.

[1] Antonelli, G., Underwater Robots: Motion and Force Control of Vehicle-Manipulator Systems, Springer, Berlin, Germany, 2006.
[2] ArduPilot Development Team, ArduPilot Sub documentation, ArduPilot Sub, online documentation, accessed Jul. 7, 2026.
[3] Åström, K. J., and Hägglund, T., PID Controllers: Theory, Design, and Tuning, Instrument Society of Automation, Research Triangle Park, NC, USA, 1995.
[4] Borenstein, J., and Koren, Y., The vector field histogram: Fast obstacle avoidance for mobile robots, IEEE Transactions on Robotics and Automation, 7(3), 278-288, 1991.
[5] Breivik, M., and Fossen, T. I., Principles of guidance-based path following in 2D and 3D, Proceedings of the 44th IEEE Conference on Decision and Control, Seville, Spain, 627-634, 2005.
[6] Caharija, W., Pettersen, K. Y., Bibuli, M., Calado, P., Zereik, E., Braga, J., Gravdahl, J. T., Sørensen, A. J., Milovanović, M., and Bruzzone, G., Integral line-of-sight guidance and control of underactuated marine vehicles: Theory, simulations, and experiments, IEEE Transactions on Control Systems Technology, 24(5), 1623-1642, 2016.
[7] Fairfield, N., Kantor, G., and Wettergreen, D., Real-time SLAM with octree evidence grids for exploration in underwater tunnels, Journal of Field Robotics, 24(1-2), 3-21, 2007.
[8] Fossen, T. I., Handbook of Marine Craft Hydrodynamics and Motion Control, 2nd ed., Wiley, Chichester, U.K., 2021.
[9] Gafurov, S. A., and Klochkov, E. V., Autonomous unmanned underwater vehicles development tendencies, Procedia Engineering, 106, 141-148, 2015.
[10] Kalman, R. E., A new approach to linear filtering and prediction problems, Journal of Basic Engineering, 82(1), 35-45, 1960.
[11] Khatib, O., Real-time obstacle avoidance for manipulators and mobile robots, The International Journal of Robotics Research, 5(1), 90-98, 1986.
[12] Kinsey, J. C., Eustice, R. M., and Whitcomb, L. L., A survey of underwater vehicle navigation: Recent advances and new challenges, Proceedings of the IFAC Conference of Manoeuvring and Control of Marine Craft, Lisbon, Portugal, 2006.
[13] Koren, Y., and Borenstein, J., Potential field methods and their inherent limitations for mobile robot navigation, Proceedings of the IEEE International Conference on Robotics and Automation, Sacramento, CA, USA, 1398-1404, 1991.
[14] Koubâa, A., Allouch, A., Alajlan, M., Javed, Y., Belghith, A., and Khalgui, M., Micro Air Vehicle Link (MAVLink) in a nutshell: A survey, IEEE Access, 7, 87658-87680, 2019.
[15] LaValle, S. M., Planning Algorithms, Cambridge University Press, Cambridge, U.K., 2006.
[16] Leith, D. J., and Leithead, W. E., Survey of gain-scheduling analysis and design, International Journal of Control, 73(11), 1001-1025, 2000.
[17] Macenski, S., Foote, T., Gerkey, B., Lalancette, C., and Woodall, W., Robot Operating System 2: Design, architecture, and uses in the wild, Science Robotics, 7(66), eabm6074, 2022.
[18] Macenski, S., Soragna, A., Carroll, M., and Ge, Z., Impact of ROS 2 node composition in robotic systems, arXiv preprint arXiv:2305.09933, 2023.
[19] Mane, P., George, A. J., Makam, R., Majumder, R., and Sundaram, S., EROAS: 3D efficient reactive obstacle avoidance system for autonomous underwater vehicles using 2.5D forward-looking sonar, arXiv preprint arXiv:2411.05516, 2024.
[20] MAVLink Development Team, MAVLink packet serialization, MAVLink Developer Guide, online documentation, accessed Jul. 7, 2026.
[21] MAVROS Development Team, MAVROS documentation, MAVROS, online documentation, accessed Jul. 7, 2026.
[22] Morency, C., and Stilwell, D. J., Evaluating the benefit of using multiple low-cost forward-looking sonar beams for collision avoidance in small AUVs, arXiv preprint arXiv:2210.06537, 2022.
[23] Morency, C., Stilwell, D. J., and Krauss, S. T., Use of a low-cost forward-looking sonar for collision avoidance in small AUVs, analysis and experimental results, arXiv preprint arXiv:2309.05785, 2023.
[24] Morgado, M., Batista, P., Oliveira, P., and Silvestre, C., Position USBL/DVL sensor-based navigation filter in the presence of unknown ocean currents, Automatica, 47(12), 2604-2614, 2011.
[25] Newman, P., and Leonard, J., Pure range-only sub-sea SLAM, Proceedings of the IEEE International Conference on Robotics and Automation, Taipei, Taiwan, 1921-1926, 2003.
[26] Olivastri, E., Fusaro, D., Li, W., Mosco, S., and Pretto, A., A sonar-based AUV positioning system for underwater environments with low infrastructure density, arXiv preprint arXiv:2405.01971, 2024.
[27] Paull, L., Saeedi, S., Seto, M., and Li, H., AUV navigation and localization: A review, IEEE Journal of Oceanic Engineering, 39(1), 131-149, 2014.
[28] Petres, C., Pailhas, Y., Patron, P., Petillot, Y., Evans, J., and Lane, D., Path planning for autonomous underwater vehicles, IEEE Transactions on Robotics, 23(2), 331-341, 2007.
[29] QGroundControl Development Team, QGroundControl User Guide, QGroundControl, online documentation, accessed Jul. 7, 2026.
[30] Williams, S. B., Newman, P., Dissanayake, G., and Durrant-Whyte, H., Autonomous underwater simultaneous localisation and map building, Proceedings of the IEEE International Conference on Robotics and Automation, San Francisco, CA, USA, 1793-1798, 2000.
[31] Wynn, R. B., Huvenne, V. A. I., Le Bas, T. P., Murton, B. J., Connelly, D. P., Bett, B. J., Ruhl, H. A., Morris, K. J., Peakall, J., Parsons, D. R., Sumner, E. J., Darby, S. E., Dorrell, R. M., and Hunt, J. E., Autonomous underwater vehicles (AUVs): Their past, present and future contributions to the advancement of marine geoscience, Marine Geology, 352, 451-468, 2014.
[32] Yuh, J., Design and control of autonomous underwater robots: A survey, Autonomous Robots, 8, 7-24, 2000.