AUV 增量PID轨迹跟踪 水下机器人无人船无人艇 USV路径跟随 MATLAB仿真

一、整体项目概述

本项目包含两大核心模块，分别是自主水下航行器（AUV）的增量PID轨迹跟踪控制模块与无人水面艇（USV）的局部风险避障模块。前者基于MATLAB实现，通过增量PID算法结合四阶龙格-库塔法求解AUV动力学与运动学方程，实现对期望轨迹的精准跟踪；后者基于C++ MFC框架开发，融合粒子群优化（PSO）算法，可处理静态、动态及未知障碍物，完成路径规划与避障功能，整体为水下及水面无人航行器的运动控制与路径规划提供完整技术方案。

二、AUV增量PID轨迹跟踪控制模块

（一）模块核心功能

该模块旨在实现AUV在存在海流干扰的环境下，对预设轨迹（如圆形轨迹）的稳定跟踪。通过增量PID控制器实时调整AUV的推力和舵力矩，修正速度与艏向角误差，结合动力学模型计算下一时刻状态，确保AUV按期望路径航行。

（二）关键组件与功能实现

1. AUV水平动力学与运动学模型（Infante_horizontal.m）
   - 状态变量定义：接收AUV当前状态向量x，包含纵向速度ur、横向速度vr、艏向角速度r、X轴位置xx、Y轴位置yy、艏向角psi，全面反映AUV运动状态。
   - 海流影响处理：根据输入的海流大小Cur与方向Dir，计算海流在AUV本体坐标系下的速度分量uc和vc，并叠加到AUV自身速度ur、vr上，得到真实运动速度u、v，精准模拟海洋环境干扰。
   - 水动力系数配置：内置AUV的质量m、转动惯量Iz及各类水动力系数（如Xdu、Ydv等），涵盖粘性阻力、附加质量等影响因素，为动力学计算提供基础参数。
   - 微分方程求解：基于牛顿运动定律，建立AUV动力学微分方程，计算纵向速度、横向速度、艏向角速度的变化率dydt(1)-dydt(3)；同时通过运动学方程，结合真实速度与艏向角，计算位置和艏向角的变化率dydt(4)-dydt(6)，输出AUV状态变化率向量。
2. 主控制逻辑（main.m）
   - 仿真参数初始化：设置仿真步长h、总运行时间对应的迭代次数m、海流参数（大小Cur、方向Dir）、PID控制器参数（推力控制Kp/Ki/Kd、力矩控制Kp1/Ki1/Kd1）及期望运动状态（期望速度ud、初始期望艏向psid），同时定义圆形期望轨迹的圆心坐标originx/originy与半径radius。
   - 四阶龙格-库塔法状态更新：在每次迭代中，调用Infante_horizontal.m函数，分别计算四阶龙格-库塔法的四个增量k1-k4，通过加权求和更新AUV当前状态向量x，相比欧拉法显著提升状态计算精度，确保动力学与运动学方程求解的准确性。
   - 增量PID控制算法：
   - 速度控制：计算当前速度误差ue（期望速度ud与实际纵向速度Y(i,1)差值），结合历史误差ue1/ue2，通过增量PID公式更新推力For，实现对AUV纵向速度的实时调节，保证其稳定跟踪期望速度。
   - 艏向角控制：计算当前艏向误差psie（期望艏向psid与实际艏向Y(i,6)差值），并对误差进行范围限制（确保在-pi到pi之间），结合历史误差psie1/psie2，通过增量PID公式更新舵力矩Tor，修正AUV艏向角，使其朝向期望运动方向。
   - 期望艏向动态调整：根据AUV当前位置与期望圆形轨迹的偏差emix，计算轨迹切线方向对应的期望艏向psid。当偏差在预设范围DETA内时，通过三角函数调整psid，引导AUV向期望轨迹靠拢；当偏差超出范围时，将psid设置为指向轨迹圆心方向，实现轨迹跟踪的动态修正。
   - 数据存储与可视化准备：实时存储AUV的推力Force、力矩Torque、状态变量Y（速度、位置、艏向角）、轨迹误差Error及期望艏向PSID，并对艏向角进行范围限制（确保在0-2*pi之间），为后续轨迹可视化（如实际航迹与期望航迹对比）提供数据支持。

（三）模块核心优势

1. 抗干扰能力：明确考虑海流对AUV运动的影响，通过速度叠加准确计算真实运动状态，使控制算法更贴合实际海洋环境。
2. 高精度控制：结合四阶龙格-库塔法与增量PID算法，既保证了状态求解的精度，又能通过PID实时修正误差，实现AUV速度与轨迹的稳定跟踪。
3. 动态轨迹适应：根据AUV与期望轨迹的偏差动态调整期望艏向，避免因固定艏向导致的轨迹偏移，提升轨迹跟踪的灵活性与准确性。

三、USV局部风险避障模块

（一）模块核心功能

基于C++ MFC框架开发，实现USV在复杂水域环境中的局部风险避障。支持对静态障碍物（圆形、矩形）、动态障碍物（圆形、矩形）及未知障碍物的识别与处理，通过PSO算法优化路径，结合海事避碰规则，规划出安全、高效的航行路径，同时提供障碍物参数配置、数据导入导出及路径可视化等交互功能。

（二）关键组件与功能实现

1. MFC界面框架组件
   - 主框架（MainFrm.cpp/.h）：构建USV应用程序的主窗口框架，包含功能区（Ribbon Bar）、状态栏、导航面板（Outlook Bar）及标题栏。导航面板集成文件目录树mwndTree与日历控件mwndCalendar，支持界面布局的动态调整（如窗口大小、停靠模式），并提供应用外观风格切换功能（如Windows 7、Office 2007等风格），提升用户交互体验。
   - 子框架（ChildFrm.cpp/.h）：实现多文档界面（MDI）的子窗口管理，支持打印、打印预览等文档操作，为路径规划结果的展示与输出提供窗口载体。
   - 日历控件（CalendarBar.cpp/.h）：自定义日历栏控件，集成CMonthCalCtrl日历控件与图像列表CImageList，实现日历显示、焦点切换及自定义绘制功能，可用于记录或调度USV的航行任务时间。
2. 障碍物参数配置模块
   - 静态圆形障碍物（SetStaticObstacleCircle.cpp/.h）：提供对话框界面，支持用户输入静态圆形障碍物的坐标（X/Y）、半径R，并通过列表控件LISTStaticObstacle展示已配置障碍物信息。支持障碍物数据的导入（从Excel文件）与导出（保存为Excel文件），同时自动计算障碍物相对于USV起点的极径与极角，并按极径排序，为路径规划中的障碍物规避计算提供基础数据。
   - 静态矩形障碍物（SetStaticObstacleRectangle.cpp/.h）：提供对话框界面，支持用户输入静态矩形障碍物的中心点坐标（CenterX/CenterY）、半长HalfLength、半宽HalfWidth及方向角DirectionA。通过CalculateStaticOR函数计算矩形四个顶点的全局坐标与极坐标（含扩展安全区域），并对顶点按极径排序；同时支持障碍物数据的导入导出，以及判断点与矩形的位置关系（calculatepointInRC函数），为路径规划中矩形障碍物的规避提供数据支持。
   - 动态障碍物（SetDynamicOC.cpp/.h、SetDynamicOR.cpp/.h）：分别针对动态圆形与矩形障碍物，提供参数配置对话框，支持输入障碍物的位置、尺寸、运动方向角DA及速度V。自动计算障碍物的极坐标、运动轨迹相关参数，并支持数据导入导出，同时结合USV运动状态判断碰撞风险，为动态避障算法提供实时障碍物运动数据。
   - 未知障碍物（SetUnknown.cpp/.h、SetUnknownOC.cpp/.h、SetUnknownOR.h）：支持配置未知障碍物的探测半径UnknownR、探测时间UnknownT等参数，针对未知圆形障碍物，通过calculate_unknownOC函数计算其扩展运动区域（考虑运动速度与时间），为USV的未知障碍物规避提供预判数据；未知矩形障碍物模块则提供类似的参数配置与位置计算功能，增强USV对未知环境的适应能力。
3. PSO路径优化与避障算法
   - 粒子类（Particle.cpp/.h）：定义粒子群优化算法中的粒子结构，每个粒子代表一条潜在路径，包含路径点数组pathpoint（存储路径点的极径pm、极角pa）、个体最优路径点pathpointBest、速度V及适应度Fitness。实现粒子初始化（setpathpoint，基于正态分布生成初始路径点，避开障碍物禁入区域）、适应度计算（calculateFitness，以路径总长度为优化目标，路径越短适应度越优）、速度与位置更新（setV，结合个体最优与全局最优路径，通过PSO公式更新路径点极角，同时判断路径有效性，避开障碍物）等核心功能。
   - 动态障碍物粒子类（ParticleDO.cpp/.h）：针对动态障碍物避障场景，定义专用粒子结构，粒子状态包含角度变化量DAchange与速度变化量Vchange。实现动态路径的适应度计算（calculateFitness，以角度与速度变化量最小为目标，保证USV运动平稳）、动态障碍物规避判断（judgePATH_effectivity，结合海事避碰规则，判断路径是否与动态障碍物碰撞）、速度与位置更新（setV，基于PSO算法优化动态避障路径）等功能，确保USV在动态环境中安全航行。
4. PSO算法参数配置（SetPSO.cpp/.h、SetDoPSO.cpp/.h）：
   - 静态路径PSO配置：通过对话框设置PSO算法的迭代次数PSOgenerations、粒子数量PSOparticlenum、最大速度尝试次数PSOsetVnumMAX及初始速度PSOsetInitialV，参数实时更新到全局PSO配置结构体pso，为静态路径优化提供算法参数支持。
   - 动态避障PSO配置：设置动态避障场景下PSO的迭代次数、粒子数量，以及速度与角度变化量的权重WeightV/WeightDa，参数更新到pso_do结构体，确保动态路径优化的算法性能与避障效果平衡。
5. 视图大小配置（SetViewSize.cpp/.h）：提供对话框界面，支持用户设置USV航行场景的视图宽度setviewL与高度setviewW，并可选择是否锁定视图大小（SetViewChange），视图大小参数存储到viewsize结构体，用于界面绘制与航行场景的可视化范围控制。

（三）模块核心优势

1. 多类型障碍物支持：全面覆盖静态、动态、未知三大类障碍物，且包含圆形、矩形两种形状，满足复杂水域环境的避障需求。
2. 智能路径优化：基于PSO算法，以路径长度最短、运动变化量最小为目标，同时结合障碍物禁入区域与海事避碰规则，规划出安全且高效的路径。
3. 完善的交互与数据管理：提供直观的MFC界面，支持障碍物参数的可视化配置、数据导入导出，方便用户操作与数据共享，同时支持视图大小调整，提升航行场景可视化效果。
4. 海事规则融合：在动态避障中考虑海事避碰规则（如左侧/右侧通行），使USV的避障行为更符合实际航行规范，增强实用性与安全性。

四、两大模块协同与应用场景

AUV增量PID轨迹跟踪模块与USV局部风险避障模块虽针对不同航行器，但在技术原理与应用场景上可形成互补与协同。例如，在海洋探测任务中，USV可作为水面控制平台，通过局部风险避障模块规划安全的水面路径，同时为水下的AUV提供位置引导与环境信息（如海面海流数据）；AUV则基于USV提供的信息，通过增量PID模块实现水下精准轨迹跟踪，完成水下探测任务。

AUV 增量PID轨迹跟踪 水下机器人无人船无人艇 USV路径跟随 MATLAB仿真

两大模块分别适用于不同场景：AUV模块适用于水下封闭环境、对轨迹精度要求高的任务（如水下管道检测、海底地形测绘）；USV模块适用于水面开放环境、障碍物复杂的任务（如水上搜救、水域巡逻），共同为海洋无人航行器的运动控制与路径规划提供技术支撑。