MSS船舶与自主系统制导导航控制(GNC)工具箱

资 源 简 介

本项目是一个综合性的MATLAB工具箱，专注于制导、导航和控制（GNC）系统的设计、仿真与验证。该工具箱为海洋船舶、水下航行器及其他自主系统的运动控制提供了完整的解决方案。主要功能涵盖了从六自由度（6-DOF）运动学与动力学数学建模，到复杂环境干扰（如风、浪、流）的数值模拟。在制导方面，项目实现了视线法（LOS）及基于路径的制导算法，支持直线、圆弧及任意曲线的路径跟踪；在导航方面，集成了扩展卡尔曼滤波（EKF）及非线性观测器，用于精准的状态估计与波浪滤波；在控制方面，提供了PID控制、反步法、滑模控制等多种控制策略，适用于航向保持、动态定位（DP）及轨迹跟踪任务。此外，工具箱还包含了一系列用于坐标系转换、船舶参数辨识及GNC系统快速原型开发的辅助函数库，能够显著缩短算法从理论到实现的开发周期。

详 情 说 明

MSS GNC 制导导航控制工具箱

项目介绍

本项目是一个基于 MATLAB 的综合性仿真工具箱，专注于海洋船舶的制导、导航与控制（GNC）系统的设计与验证。项目核心围绕六自由度（6-DOF）船舶运动模型展开，集成了一整套从环境干扰模拟、传感器噪声处理、状态估计到路径跟踪控制的闭环仿真环境。

该工具箱实际模拟了一艘通用供给船在复杂海洋环境下的自主航行过程，通过视线法（LOS）实现路径跟踪，利用扩展卡尔曼滤波（EKF）进行状态观测，并采用经典的 PID 控制策略维持航向与航速。

主要功能特性

• 6-DOF 船舶动力学仿真：基于 Fossen 模型，实现了包含刚体惯性、附加质量、非线性阻尼（线性+二次项）、科里奥利力及恢复力的完整六自由度运动方程。
• 环境干扰模拟：支持模拟风、浪、流对船舶的联合作用，包括流速/流向、风速/风向及波浪高度并在仿真回路中实时计算干扰力。
• 导航系统（GNS）：内置扩展卡尔曼滤波器（EKF），能够处理带有高斯白噪声的 GPS 和 IMU 模拟测量数据，输出平滑的位置、航向及速度估计值。
• 制导系统（Guidance）：实现了基于视线法（LOS）的路径跟踪算法，支持多航点（Waypoints）自动切换，并具备自适应速度调度功能。
• 运动控制（Control）：实现了用于航速控制的 PI 控制器和用于航向保持的 PD 控制器，并包含执行器推力饱和逻辑。
• 可视化分析：记录并存储仿真全过程的状态数据（位置、速度、控制量、环境力等），并在仿真结束后调用可视化模块展示结果。

实际实现逻辑与算法细节

以下内容基于 main.m 源代码的实际执行逻辑编写：

1. 仿真架构

主程序运行一个基于时间步长（0.05s）的离散仿真循环，总仿真时长默认为 600 秒。循环内部严格遵循 导航 -> 制导 -> 控制 -> 物理模型求解 的 GNC 处理流程。

2. 导航模块 (Navigation)

• 传感器模拟：在真实位置数据上叠加了随机噪声，模拟 GPS（北向/东向位置）和 IMU（航向角）的测量输出。
• EKF 状态估计：

* 预测步骤：基于非线性船舶运动学模型预测下一时刻状态，采用了简化的 3-DOF（纵荡、横荡、艏摇）动力学模型以提高计算效率。 * 更新步骤：利用线性化雅可比矩阵计算卡尔曼增益，结合测量残差修正状态向量 [North, East, Psi, u, v, r]。 * 输出：提供滤波后的位置和速度信息供控制系统使用。

3. 制导模块 (Guidance)

• 航点管理：定义了一组预设的经纬度航点（Waypoints），设定了 20 米的航点切换半径（Acceptance Radius）。
• LOS 算法：

* 计算船舶当前位置相对于目标路径的横向误差（Cross-track error）。 * 基于前视距离（3倍船长）计算期望的切向航向角，引导船舶收敛至路径。 * 速度调度：当横向误差较大（>50m）时，自动降低期望航速至 3.0 m/s 以减小超调；正常巡航速度为 5.0 m/s。

4. 控制模块 (Control)

• 纵向速度控制：采用 PI（比例-积分）控制器。积分项用于消除由环境阻力导致的速度静差。
• 艏摇航向控制：采用 PD（比例-微分）控制器。利用航向误差和角速度阻尼来维持期望航向。
• 执行器模型：包含推力分配逻辑（简化为纵向推力和艏摇力矩），并经过饱和函数处理，限制最大输出推力和力矩，模拟物理执行器的极限。

5. 物理场建模 (Plant Dynamics)

• 动力学解算：采用欧拉法（Euler Integration）求解微分方程 $Mdot{nu} + C(nu)nu + D(nu)nu + g(eta) = tau + tau_{env}$。
• 矩阵计算：

* M 矩阵：包含刚体质量和附加质量。 * D 矩阵：结合了线性阻尼和与速度相关的二次非线性阻尼。 * C 矩阵：计算科里奥利向心力矩阵（源代码中包含该函数，但实际片段在末尾截断）。 * J 矩阵：计算从船体坐标系（Body Frame）到导航坐标系（NED）的旋转矩阵，处理欧拉角变换。

关键函数说明

• init_vessel_model: 初始化通用供给船的物理参数，包括质量分布、几何尺寸、最大推力限制以及恢复力系数。
• guidance_los: 计算视线法制导律，输出期望航向角和期望纵向速度，并计算横向偏差。
• nav_ekf: 扩展卡尔曼滤波器的核心实现。包含状态转移矩阵的构建、雅可比矩阵的线性化、协方差矩阵的迭代以及测量更新过程。
• actuator_saturation: 对控制指令进行限幅处理，防止输出超过推进器的物理能力。
• euler_angle_rotation: 生成 6-DOF 的运动学变换矩阵，用于计算位置和姿态的微分变化。
• damping_matrix: 根据当前船体速度动态构建非线性阻尼矩阵。
• coriolis_matrix: 计算包含刚体项和附加质量项的科里奥利力矩阵（用于处理旋转坐标系下的惯性力）。

使用与系统要求

系统要求

• MATLAB (推荐 R2018b 或更高版本)
• 无需额外工具箱，所有算法均为原生 m 代码实现。

运行方法

直接在 MATLAB 中运行 main 函数即可启动仿真。仿真过程中会输出进度日志，结束后自动调取可视化函数展示轨迹对比、状态误差及控制量曲线。

注意：主程序依赖 visualize_results 和 compute_environment 函数（在提供的代码片段中被调用），请确保这些辅助函数在路径中可用。