【MPC】【USV】无人水面艇轨迹跟踪与避障的实时非线性模型预测控制研究（Matlab代码实现）

最新推荐文章于 2026-08-22 09:38:40 发布

原创最新推荐文章于 2026-08-22 09:38:40 发布 · 25 阅读

0 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文内容如下：🎁🎁🎁

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能解答你胸中升起的一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥第一部分——内容介绍

无人水面艇轨迹跟踪与避障的实时非线性模型预测控制研究

摘要

随着环境监测、搜救等海洋活动需求的增加，无人水面艇（USV）研究受到广泛关注。运动稳定与跟踪控制技术是USV需要解决的基础问题。此外，控制问题中还需考虑避障以确保安全性。本文提出了一种针对约束三自由度（3-DOF）动力学USV的实时非线性模型预测控制（NMPC）方案。导航中的障碍物规避问题被转化为NMPC中的可变约束问题，而《国际海上避碰规则》（COLREGS）中的船舶避让规则则通过代价函数中的权重值来实现。该带约束的优化问题由开源高效优化软件CasADi在有限时域和有限风险最小化框架下求解。本文以一艘配备双电动推进模块的波浪自适应模块化USV为验证对象，在MATLAB中模拟了三种航行场景：定点射击、轨迹跟踪以及COLREGS第14条规定的对遇局面。结果表明，所提方法具有良好的控制性能和实时性要求。

关键词

无人水面艇；轨迹跟踪；避障控制；非线性模型预测控制；实时控制；COLREGS规则

1 引言

1.1 研究背景与意义

随着海洋开发、环境监测、海上搜救、港口安防等海洋相关活动的日益频繁，无人水面艇（Unmanned Surface Vehicle, USV）作为一种无需人员驾驶、可自主完成各类海上任务的小型水面平台，凭借其机动性强、成本较低、无人员伤亡风险、适配复杂恶劣海洋环境等优势，成为海洋工程领域的研究热点与应用重点。USV的自主运行能力直接决定了其任务执行的安全性与效率，而轨迹跟踪与避障控制是实现USV自主运行的核心技术支撑，也是保障其在复杂海洋环境中稳定作业的关键前提。

在实际海洋场景中，USV需在完成预设轨迹跟踪任务的同时，应对各类静态障碍物（如暗礁、浮标、废弃船只）与动态障碍物（如其他航行船舶、漂浮物）的干扰，且需遵循《国际海上避碰规则》（International Regulations for Preventing Collisions at Sea, COLREGS），确保航行安全与合规性。然而，USV的运动过程具有显著的非线性动力学特性，受波浪、水流、风力等海洋环境扰动的影响较大，且其推进系统、转向系统存在物理约束，传统控制方法难以兼顾轨迹跟踪精度、避障实时性与规则合规性，无法满足复杂场景下的自主控制需求。

模型预测控制（Model Predictive Control, MPC）作为一种基于动态模型的先进优化控制策略，凭借其能够显式处理多约束、多目标优化问题的独特优势，在USV控制领域得到广泛关注。其中，非线性模型预测控制（Nonlinear Model Predictive Control, NMPC）能够更精准地描述USV的非线性动力学行为，避免线性化近似带来的控制误差，更适用于复杂海洋环境下的USV控制场景。但NMPC存在计算复杂度较高的问题，如何在保证控制性能的前提下，提升其实时性，使其能够满足USV轨迹跟踪与避障的实时控制需求，成为当前研究的核心难点之一。

基于此，本文针对约束三自由度动力学USV，提出一种实时NMPC控制方案，将避障问题与COLREGS规则融入控制设计中，通过高效优化求解实现轨迹跟踪与避障的协同控制，为USV的自主安全运行提供技术支撑，具有重要的理论研究价值与工程应用意义。

1.2 国内外研究现状

近年来，国内外学者围绕USV轨迹跟踪与避障控制开展了大量研究，形成了多种控制方法。传统控制方法中，比例-积分-微分（PID）控制因其结构简单、易于实现，被广泛应用于USV轨迹跟踪控制，但该方法对非线性系统与环境扰动的适应性较差，难以处理多约束问题，在复杂避障场景下控制性能不佳。线性二次型调节器（LQR）通过线性化系统模型实现最优控制，但同样无法精准匹配USV的非线性动力学特性，在大范围轨迹跟踪与动态避障场景中存在明显局限性。

随着MPC技术的发展，其在USV控制中的应用研究不断深入。早期研究多采用线性模型预测控制（LMPC），通过线性化USV动力学模型简化优化问题，提升计算效率，但线性化近似会导致控制精度下降，尤其在高速转向、强扰动等非线性较强的场景中，控制效果不理想。为解决这一问题，NMPC被逐步应用于USV轨迹跟踪与避障控制，其能够直接基于USV的非线性动力学模型进行优化，有效提升控制精度。

目前，国内外关于USV的NMPC控制研究主要集中在避障策略设计与计算效率优化两个方面。在避障策略方面，部分研究将避障问题转化为约束条件，通过设置障碍物区域的约束，实现轨迹跟踪与避障的协同控制，但多数研究未充分考虑COLREGS规则，导致USV在实际航行中可能存在避障行为不合规的问题。在计算效率优化方面，学者们通过简化优化问题、改进优化算法、采用高效优化工具等方式，降低NMPC的计算复杂度，提升实时性，但现有方法在兼顾控制性能与实时性方面仍存在不足，难以满足复杂动态场景下的实时控制需求。

此外，现有研究中，部分方案采用的USV模型简化程度较高，未充分考虑推进系统约束与环境扰动的影响，导致控制方案的实际适配性较差。针对上述问题，本文提出一种融合COLREGS规则的实时NMPC控制方案，优化避障约束设计与代价函数构建，采用高效优化工具求解，提升控制性能与实时性，同时基于实际USV模型进行仿真验证，增强方案的工程实用性。

1.3 研究内容与技术路线

本文围绕USV轨迹跟踪与避障的实时NMPC控制展开研究，主要研究内容如下：

1. 分析USV的运动特性与控制需求，明确三自由度动力学模型的核心约束，包括推进系统物理约束、运动状态约束等，为控制方案设计奠定基础；

2. 设计实时NMPC控制框架，将障碍物规避问题转化为NMPC中的可变约束，结合COLREGS规则设计代价函数，实现轨迹跟踪精度、避障安全性与规则合规性的多目标优化；

3. 采用开源高效优化软件CasADi，在有限时域与有限风险最小化框架下，求解带约束的非线性优化问题，优化计算流程，提升控制实时性；

4. 以配备双电动推进模块的波浪自适应模块化USV为验证对象，在MATLAB中构建三种典型航行场景，进行仿真实验，验证所提控制方案的有效性与实时性。

本文的技术路线为：首先明确研究背景与研究难点，梳理国内外研究现状，确定研究内容与目标；其次，分析USV动力学特性与控制约束，构建NMPC控制框架，设计避障约束与代价函数；然后，采用CasADi软件实现优化问题求解，优化计算效率；最后，通过MATLAB仿真实验，验证控制方案的性能，并总结研究结论与未来展望。

1.4 论文结构安排

本文共分为6章，具体结构安排如下：第1章为引言，阐述研究背景与意义、国内外研究现状、研究内容与技术路线，明确本文的研究重点；第2章为USV动力学特性与控制约束分析，介绍USV的运动特性，明确三自由度动力学模型的核心约束与控制需求；第3章为实时NMPC控制方案设计，包括控制框架构建、避障约束设计、COLREGS规则融入与代价函数设计；第4章为优化问题求解与实时性优化，介绍CasADi软件的应用，阐述有限时域与有限风险最小化框架下的优化求解流程；第5章为仿真实验与结果分析，构建三种航行场景，进行仿真验证，分析控制性能与实时性；第6章为结论与展望，总结本文的研究成果，分析研究中存在的不足，并对未来研究方向进行展望。

2 USV动力学特性与控制约束分析

2.1 USV运动特性概述

无人水面艇的运动主要发生在水平平面内，其核心运动状态包括位置、航向角、速度等，通常采用三自由度（3-DOF）动力学模型描述其平面运动，忽略垂直方向的横摇、纵摇等运动，能够精准反映USV在实际航行中的主要运动特性。USV的运动过程受自身动力学特性与外部环境扰动的双重影响，具有显著的非线性、强耦合特性。

从自身动力学特性来看，USV的推进系统与转向系统存在强耦合关系，例如双电动推进模块的转速差不仅会影响航速，还会改变航向角，高速转向时艇体的姿态变化会进一步影响水动力系数，导致运动状态的非线性变化。同时，水动力参数（如阻力系数、附加质量）会随航速、吃水深度等因素动态变化，难以通过机理建模精确描述，增加了控制难度。

从外部环境扰动来看，海洋环境中的波浪、水流、风力等都会对USV的运动产生干扰。波浪会导致艇体的小幅晃动，影响运动稳定性；水流会产生侧向漂移，尤其是在低速航行时，水流干扰可能导致USV偏离预设轨迹；风力会对艇体产生推力矩，改变航向与航速，这些扰动都会降低轨迹跟踪精度，增加避障控制的难度。

本文研究的USV为波浪自适应模块化设计，配备双电动推进模块，具有良好的机动性与环境适应性，能够在复杂海洋环境中完成轨迹跟踪与避障任务，其核心运动特性与双电动推进系统的控制逻辑，为后续NMPC控制方案的设计提供了重要依据。

2.2 USV控制约束分析

USV的轨迹跟踪与避障控制需满足多种约束条件，主要包括物理约束、任务约束与安全约束三类，这些约束是NMPC控制方案设计的重要前提，也是确保控制方案可行性与安全性的关键。

物理约束主要源于USV自身的硬件性能限制，包括推进系统约束与运动状态约束。推进系统约束方面，双电动推进模块的转速存在上限，无法实现无限大的推力，同时推进电机的响应速度存在限制，导致控制输入无法突变；运动状态约束方面，USV的航速、转向角速度存在最大值，航向角的变化范围也受到限制，避免因剧烈运动导致艇体失稳或侧翻。此外，USV的横倾角需控制在安全范围内，防止因姿态失衡影响航行安全。

任务约束主要基于USV的作业需求，本文主要针对轨迹跟踪与避障任务，约束条件包括轨迹跟踪的精度要求、避障的安全距离要求等。例如，在高精度轨迹跟踪任务中，USV的位置偏差需控制在合理范围内；在避障任务中，USV与障碍物之间需保持足够的安全距离，避免碰撞事故发生。

安全约束主要包括COLREGS规则约束与避障安全约束。COLREGS规则是国际海上航行的基本准则，明确了船舶在相遇、交叉相遇等场景下的避让责任与行动要求，USV作为海上航行载体，需严格遵循该规则，确保避障行为合规。避障安全约束则要求USV在遇到障碍物时，能够及时调整轨迹，避免与静态或动态障碍物发生碰撞，同时确保调整后的轨迹能够回归预设轨迹，不影响任务执行。

2.3 控制需求分析

结合USV的运动特性与控制约束，其轨迹跟踪与避障控制的核心需求主要包括三个方面：一是轨迹跟踪精度，USV需能够精准跟踪预设轨迹，减小位置偏差与航向偏差，确保任务顺利执行；二是避障实时性，在遇到障碍物时，需快速做出响应，调整控制策略，实现安全避障，避免因响应延迟导致碰撞；三是规则合规性，避障行为需严格遵循COLREGS规则，确保USV在实际航行中的安全性与合规性。

此外，控制方案还需具备良好的抗干扰能力，能够应对波浪、水流等环境扰动的影响，保持运动稳定；同时，需兼顾计算效率与控制性能，在复杂场景下能够快速求解优化问题，满足实时控制需求。传统控制方法难以同时满足上述需求，而NMPC能够通过显式处理多约束、多目标优化问题，精准匹配USV的非线性动力学特性，成为实现上述控制需求的理想方案。

3 实时非线性模型预测控制方案设计

3.1 NMPC控制框架构建

本文提出的实时NMPC控制框架以USV的三自由度非线性动力学模型为基础，采用“预测-优化-反馈”的闭环控制逻辑，实现轨迹跟踪与避障的协同控制。该框架主要包括预测模型、滚动优化、反馈校正三个核心模块，同时融入避障约束与COLREGS规则，形成完整的控制体系。

预测模型是NMPC控制的基础，用于预测USV在未来有限时域内的运动状态。本文基于USV的三自由度非线性动力学特性，构建预测模型，能够精准反映USV的位置、航向角、速度等状态的变化规律，同时考虑波浪、水流等环境扰动的影响，提升预测精度。预测模型的输入为USV的控制量（如推进模块转速、转向角度），输出为未来有限时域内的预测状态序列。

滚动优化是NMPC控制的核心环节，通过在每个控制周期内求解带约束的非线性优化问题，生成最优控制序列。本文的滚动优化以轨迹跟踪精度、避障安全性、规则合规性为优化目标，结合USV的物理约束、任务约束与安全约束，构建优化问题，通过高效优化算法求解，得到当前控制周期的最优控制量。与传统MPC不同，本文采用有限时域优化策略，将整个控制过程划分为多个连续的有限时域，每个控制周期仅执行最优控制序列的第一个控制量，在下一个控制周期重新进行预测与优化，实现滚动控制。

反馈校正模块用于补偿预测误差与环境扰动的影响，提升控制鲁棒性。通过传感器获取USV的实际运动状态，与预测模型输出的预测状态进行对比，计算状态偏差，将偏差信息反馈至预测模型与滚动优化环节，调整预测参数与优化目标，修正控制量，确保USV能够稳定跟踪预设轨迹，实现安全避障。

3.2 避障约束设计

为实现USV的避障控制，本文将障碍物规避问题转化为NMPC中的可变约束问题，根据障碍物的位置、大小与运动状态，动态调整约束条件，确保USV在预测时域内的运动轨迹不与障碍物发生碰撞。

首先，对障碍物进行检测与建模，通过环境感知系统获取障碍物的位置信息、尺寸信息以及运动状态（静态障碍物运动状态为零），构建障碍物的安全区域。安全区域的大小根据USV的尺寸、航速以及避障安全距离确定，确保USV与障碍物之间保持足够的安全间隙，避免碰撞。

其次，将障碍物的安全区域转化为NMPC的状态约束，即约束USV在未来预测时域内的所有预测状态都不能进入障碍物的安全区域。由于障碍物的位置可能动态变化，该约束为可变约束，在每个控制周期内，根据最新的障碍物信息，更新约束条件，确保避障约束的实时性与准确性。

针对动态障碍物（如其他航行船舶），考虑其运动轨迹的不确定性，采用有限风险最小化框架，在约束设计中引入风险系数，确保USV在避障过程中能够应对动态障碍物的运动变化，降低碰撞风险。同时，结合USV的运动特性，优化约束条件的表述方式，避免约束过于严格导致优化问题无解，兼顾避障安全性与控制灵活性。

3.3 COLREGS规则融入设计

为确保USV的避障行为符合《国际海上避碰规则》，本文将COLREGS规则通过代价函数中的权重值进行量化，融入NMPC的滚动优化过程中，使USV在避障过程中自动遵循规则要求，实现合规避障。

COLREGS规则明确了不同相遇场景下船舶的避让责任与行动要求，本文重点针对COLREGS第14条规定的对遇局面，设计代价函数的权重分配策略。在对遇局面中，两船应各自向右转，从对方左舷通过，避免迎头碰撞。为实现这一规则，在代价函数中设置航向调整权重，当USV遇到对遇局面时，增大航向向右调整的权重，鼓励USV按照规则要求调整航向，同时减小航向向左调整的权重，抑制不合规的避障行为。

此外，针对其他常见的相遇场景，预留权重调整接口，可根据COLREGS规则的具体要求，调整代价函数中不同目标的权重值，实现不同场景下的合规避障。通过权重分配，将规则要求转化为优化目标的一部分，使USV在求解优化问题时，能够兼顾轨迹跟踪、避障安全与规则合规，确保避障行为的合理性与合规性。

3.4 代价函数设计

代价函数是NMPC滚动优化的核心，用于量化控制目标的优先级，本文的代价函数以轨迹跟踪精度、避障安全性、规则合规性以及控制平滑性为核心目标，通过合理的权重分配，实现多目标优化。

代价函数主要包括四个部分：轨迹跟踪误差项、避障安全项、COLREGS规则项与控制输入平滑项。轨迹跟踪误差项用于衡量USV的实际状态与预设轨迹参考状态的偏差，包括位置偏差与航向偏差，通过设置合适的权重，确保USV能够精准跟踪预设轨迹；避障安全项用于惩罚USV接近障碍物的行为，当USV的预测状态接近障碍物安全区域时，该部分代价增大，推动USV远离障碍物，实现安全避障；COLREGS规则项用于量化USV避障行为的合规性，根据规则要求设置权重，鼓励合规行为，抑制不合规行为；控制输入平滑项用于惩罚控制输入的突变，确保推进系统与转向系统的控制输入平滑变化，避免因控制输入突变导致艇体失稳，延长设备使用寿命。

通过调整各部分的权重值，可实现不同控制目标的优先级调整。例如，在障碍物密集区域，增大避障安全项的权重，优先保证避障安全；在轨迹跟踪精度要求较高的场景，增大轨迹跟踪误差项的权重，提升跟踪精度；在对遇局面等场景，增大COLREGS规则项的权重，确保避障行为合规。通过合理的权重分配，使代价函数能够适应不同的航行场景，实现多目标协同优化。

4 优化问题求解与实时性优化

4.1 优化问题描述

本文提出的NMPC控制方案中，滚动优化的核心是求解带约束的非线性优化问题。该优化问题以代价函数最小化为目标，约束条件包括USV的物理约束、避障可变约束、COLREGS规则约束以及任务约束，具体描述如下：在每个控制周期内，基于当前USV的实际状态，通过预测模型得到未来有限时域内的预测状态序列，在满足所有约束条件的前提下，求解使代价函数最小的控制序列，作为当前控制周期的最优控制输入。

该优化问题属于非线性带约束优化问题，具有变量多、约束复杂、求解难度大的特点，其求解效率直接影响NMPC的实时性。传统优化算法难以在短时间内求解该问题，无法满足USV轨迹跟踪与避障的实时控制需求，因此，本文采用开源高效优化软件CasADi求解该优化问题，结合有限时域与有限风险最小化框架，提升求解效率与稳定性。

4.2 CasADi优化软件应用

CasADi是一款开源的高效数值优化软件，支持非线性优化、最优控制等问题的求解，具有建模灵活、求解速度快、支持多种优化算法等优势，能够有效处理USV NMPC控制中的复杂非线性带约束优化问题。

本文利用CasADi软件构建优化问题的数学模型，将USV的预测模型、代价函数与约束条件转化为CasADi支持的优化格式，选择合适的优化算法进行求解。CasADi支持多种非线性优化算法，结合本文优化问题的特点，选择高效的内点法作为核心求解算法，该算法具有收敛速度快、求解精度高的优势，能够在短时间内求解复杂的非线性带约束优化问题。

同时，利用CasADi软件的自动微分功能，快速计算代价函数与约束条件的梯度信息，提升优化算法的收敛速度，进一步降低求解时间。此外，CasADi软件支持代码自动生成，能够将优化模型转化为高效的执行代码，减少求解过程中的冗余计算，提升求解效率，为NMPC的实时控制提供保障。

4.3 实时性优化策略

为进一步提升NMPC控制的实时性，满足USV轨迹跟踪与避障的实时控制需求，本文在采用CasADi软件求解的基础上，结合以下优化策略，降低计算复杂度，缩短求解时间。

首先，优化预测时域与控制时域的选择。预测时域过长会导致预测状态序列过多，增加计算量，延长求解时间；预测时域过短则会降低预测精度，影响控制性能。控制时域过长会增加控制变量的数量，同样增加计算复杂度。本文通过多次仿真实验，确定合适的预测时域与控制时域，在保证控制性能的前提下，尽量缩短时域长度，减少计算量。

其次，采用分层优化策略，将控制问题分为规划层与跟踪层。规划层主要负责生成全局避障轨迹，结合障碍物信息，快速规划出满足避障要求与COLREGS规则的大致轨迹；跟踪层负责基于规划层生成的轨迹，通过NMPC实现精准跟踪，优化控制输入。分层优化能够将复杂的优化问题分解为两个简单的子问题，降低每个子问题的计算复杂度，提升整体求解效率。

最后，优化约束条件的表述方式，简化约束计算。针对避障可变约束，采用高效的碰撞检测算法，快速判断USV预测轨迹与障碍物安全区域的位置关系，避免冗余的约束计算；针对物理约束与规则约束，简化约束条件的数学表述，减少计算量，同时确保约束的有效性。通过上述优化策略，有效缩短优化问题的求解时间，确保NMPC控制的实时性，满足USV自主航行的控制需求。

5 仿真实验与结果分析

5.1 仿真实验设置

为验证本文提出的实时NMPC控制方案的有效性与实时性，以一艘配备双电动推进模块的波浪自适应模块化USV为验证对象，在MATLAB仿真环境中构建三种典型航行场景，进行仿真实验。

首先，确定USV的核心参数，该USV采用双电动推进模块，质量、转动惯量、推进模块间距等参数参考实际波浪自适应模块化USV的设计参数，确保仿真模型的真实性。同时，设置仿真环境参数，模拟波浪、水流等环境扰动，扰动强度参考实际海洋环境的中等海况，确保仿真场景的合理性。

其次，构建三种航行场景，分别为定点射击场景、轨迹跟踪场景以及COLREGS第14条规定的对遇局面场景，覆盖USV常见的作业场景，全面验证控制方案的性能。其中，定点射击场景用于验证USV的定点控制能力，即USV从初始位置出发，快速到达指定目标点并保持稳定；轨迹跟踪场景用于验证USV的轨迹跟踪精度，预设一条复杂的曲线轨迹，要求USV精准跟踪该轨迹，同时应对环境扰动；对遇局面场景用于验证USV的避障合规性，模拟USV与另一艘船舶迎头相遇的场景，要求USV按照COLREGS第14条规则进行避障，确保避障行为合规且安全。

仿真实验中，设置控制周期与采样周期，记录USV的运动状态数据、控制输入数据以及优化求解时间，用于后续的性能分析。同时，为对比验证本文提出的NMPC控制方案的优势，引入传统PID控制方案作为对比，在相同的仿真场景与参数设置下，进行对比仿真，分析两种控制方案的轨迹跟踪精度、避障性能与实时性。

5.2 仿真结果分析

5.2.1 定点射击场景结果分析

定点射击场景中，USV从初始位置出发，目标是快速到达指定目标点并保持稳定。仿真结果表明，本文提出的NMPC控制方案能够使USV快速响应，快速调整航速与航向，朝着目标点行驶，到达目标点后能够稳定保持位置与航向，无明显振荡。相比之下，传统PID控制方案在到达目标点后，存在一定的位置振荡，调整时间较长，无法快速实现稳定控制。

通过分析USV的位置偏差数据可知，NMPC控制方案的位置偏差始终保持在较小范围内，到达目标点后的稳态偏差远小于PID控制方案，表明本文提出的控制方案具有良好的定点控制性能，能够满足USV定点作业的需求。同时，NMPC控制方案的控制输入平滑，无明显突变，避免了推进系统与转向系统的频繁调整，提升了设备运行的稳定性。

5.2.2 轨迹跟踪场景结果分析

轨迹跟踪场景中，预设一条复杂的曲线轨迹，模拟USV在实际作业中的轨迹跟踪需求，同时加入波浪、水流等环境扰动。仿真结果表明，本文提出的NMPC控制方案能够精准跟踪预设轨迹，即使在环境扰动的影响下，位置偏差与航向偏差也能够控制在合理范围内，跟踪精度较高。

对比PID控制方案，NMPC控制方案的轨迹跟踪误差显著减小，尤其是在曲线轨迹的转向段，PID控制方案出现明显的轨迹偏离，而NMPC控制方案能够快速调整控制输入，纠正轨迹偏离，确保跟踪精度。同时，NMPC控制方案能够有效抑制环境扰动的影响，保持运动稳定，而PID控制方案受扰动影响较大，轨迹跟踪精度明显下降。这表明本文提出的NMPC控制方案具有良好的轨迹跟踪性能与抗干扰能力，能够适应复杂海洋环境下的轨迹跟踪需求。

5.2.3 对遇局面场景结果分析

对遇局面场景中，模拟USV与另一艘船舶迎头相遇的场景，验证USV的避障合规性与安全性。仿真结果表明，本文提出的NMPC控制方案能够及时检测到相遇船舶，根据COLREGS第14条规则，自动调整航向，向右转舵，从对方左舷通过，实现安全避障，避障过程中无碰撞发生，且避障行为符合规则要求。

在避障过程中，USV能够在保证避障安全的前提下，尽量减少轨迹偏离，避障完成后能够快速回归预设轨迹，不影响后续任务执行。相比之下，PID控制方案在遇到对遇局面时，避障响应较慢，且避障行为未严格遵循COLREGS规则，存在碰撞风险，避障完成后轨迹回归速度较慢，影响任务效率。这表明本文提出的NMPC控制方案能够实现合规、安全、高效的避障，满足USV实际航行中的避障需求。

5.3 实时性分析

实时性是USV轨迹跟踪与避障控制的关键要求，本文通过记录仿真实验中每个控制周期的优化求解时间，分析NMPC控制方案的实时性。仿真结果表明，本文提出的NMPC控制方案，在采用CasADi软件与实时性优化策略的前提下，每个控制周期的优化求解时间均小于预设的控制周期，能够满足实时控制需求。

具体来看，在三种仿真场景中，优化求解时间均保持在较低水平，即使在障碍物密集、轨迹复杂的场景中，求解时间也未出现明显增加，表明本文采用的优化求解方法与实时性优化策略能够有效降低计算复杂度，提升求解效率。相比之下，传统NMPC控制方案的求解时间较长，无法满足实时控制需求，而本文提出的方案通过多种优化策略，显著提升了实时性，确保USV能够快速响应环境变化，实现实时轨迹跟踪与避障。

5.4 实验结论

通过三种典型航行场景的仿真实验，结合与传统PID控制方案的对比分析，可得出以下结论：本文提出的针对约束三自由度动力学USV的实时NMPC控制方案，能够有效实现轨迹跟踪与避障的协同控制，具有良好的控制性能与实时性。该方案能够精准跟踪预设轨迹，有效应对环境扰动，避障行为严格遵循COLREGS规则，安全且合规；同时，通过CasADi软件与实时性优化策略，确保了控制的实时性，能够满足USV自主航行的控制需求。