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

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

🎁完整资源、论文复现、期刊合作、论文辅导及科研仿真定制事宜点击：

👉👉👉本文完整资源下载

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

⛳️赠与读者

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

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

💥第一部分——内容介绍

基于李雅普诺夫模型预测控制的自主水下航行器轨迹跟踪控制

摘要

自主水下航行器（AUV）是海洋勘探、水下管线巡检、海洋环境监测等任务的核心装备，轨迹跟踪能力直接决定水下作业的完成质量。水下复杂流场扰动、航行器本体强耦合非线性动力学、推进器输出幅值限制等多重约束，给高精度轨迹跟踪控制器设计带来巨大挑战。传统线性控制难以适配曲线参考轨迹，常规反步控制固定增益调节方式动态响应偏弱，标准模型预测控制缺少完备稳定证明，三类主流方案均存在明显短板。

本文面向水平面运动的观测级 AUV，提出一套融合李雅普诺夫理论与模型预测控制的 LMPC 一体化跟踪控制框架。该方案将推进器推力分配优化嵌入滚动时域优化求解过程，无需单独设计推力分配模块，可同步处理推进器饱和、运动状态约束等工程硬限制。依托非线性反步控制构造李雅普诺夫收缩约束，从理论层面严格保证优化问题的递推可行性与闭环系统全局渐近稳定，完整推导系统吸引域的显式表达形式，规避传统模型预测控制依赖局部线性化的固有缺陷。滚动时域迭代的反馈结构赋予控制器天然抗扰动能力，有效抑制模型参数失配、海洋洋流带来的跟踪误差。

本文从理论层面给出三组核心创新：一是构建适配 AUV 非线性动力学的 LMPC 跟踪控制算法，同步优化跟踪精度与系统鲁棒性；二是摆脱标准 MPC 局部线性化假设，完整推导稳定性充分条件并量化系统稳定吸引范围；三是算法兼容优化次优解，可根据水下嵌入式硬件算力灵活平衡计算负荷与控制性能。基于 Saab SeaEye Falcon 水下航行器实测动力学模型完成多工况仿真测试，分别采用正弦连续轨迹、高曲率八字形轨迹开展对比试验，同时叠加 30% 模型参数偏差与恒定海流扰动完成鲁棒性验证。仿真数据表明，相较于传统反步控制，本文所提 LMPC 方案位置、艏向跟踪均方误差最高可降低 99.5%，收敛速度大幅提升，在强扰动环境下仍能维持高精度轨迹跟踪效果。

关键词：自主水下航行器；轨迹跟踪；模型预测控制；李雅普诺夫稳定理论；反步控制；推力分配；鲁棒控制

一、引言

1.1 研究背景与应用价值

海洋资源开发、水下基础设施维护、海洋生态调查等水下作业场景，对无人水下装备的自主化、高精度运动控制提出了更高要求。自主水下航行器无需人工线缆拖拽、无需操作人员实时远程操控，能够长时间潜入水下完成大范围持续性作业，可显著降低载人潜水器的作业风险与综合运营成本，近年来得到海洋工程、控制工程领域学者与海洋装备企业的广泛关注。

自主航行的核心支撑是高性能运动控制系统，其中轨迹跟踪是完成水下定点观测、管线跟随、区域巡航等任务的基础功能。航行器在水平面包含纵向、横向、艏向三组耦合运动，流体附加质量、非线性阻尼、科氏向心力会带来强非线性耦合动力学特性；同时水下洋流、海水密度分层、水底地形扰动会产生时变未知外部干扰，实际工程中推进器输出推力存在固定幅值上限，多重复杂条件叠加，大幅提升轨迹跟踪控制器设计难度。

1.2 现有控制方法研究现状

当前国内外针对 AUV 轨迹跟踪的控制方案主要分为线性控制、非线性连续控制、模型预测控制三大类，各类方法存在各自适用场景与局限性。

第一类为线性控制方法，包含比例积分微分控制、线性二次调节器等。该类控制器结构简单、参数调试便捷，仅适用于点位式分段直线跟踪任务，此时航行器运动偏差较小，系统可近似满足局部线性化假设。若目标轨迹为连续曲线、大曲率转弯轨迹，航行器大范围运动将充分激发系统非线性耦合特性，线性控制器会出现跟踪滞后、稳态误差激增甚至失稳问题，无法满足复杂水下巡航任务需求。

第二类为非线性连续控制，主流分为反步控制与滑模控制。基于李雅普诺夫的反步控制是目前水下航行器跟踪控制应用最广泛的方案，能够主动利用系统动力学内部有利非线性项，相比反馈线性化控制拥有更强的基础鲁棒性，现有研究已基于完整六自由度运动学、动力学模型设计全局镇定反步律，同时拓展出输出反馈反步控制结构。但传统反步控制器采用固定控制增益，增益选取存在天然矛盾：增大增益可加快误差收敛速度，但会缩小系统稳定吸引域；减小增益能够扩大稳定范围，却会造成动态响应缓慢、扰动抑制能力不足。滑模控制依靠滑动面对模型不确定性的不敏感特性实现抗扰控制，然而原生滑模存在高频抖振现象，会加剧推进器机械损耗；自适应滑模、高阶滑模虽可削弱抖振，但控制器参数整定流程复杂，多约束场景下无法直接处理推进器饱和限制。

第三类为模型预测控制，该方法以有限时域滚动优化为核心，能够在控制器设计阶段直接引入执行器幅值、运动状态等各类约束，是处理带约束非线性系统的理想控制框架。现有 AUV 模型预测控制研究分为线性 MPC 与非线性 MPC：线性 MPC 搭配智能优化算法求解二次规划，仅适用于低速小范围运动；非线性模型预测控制可同步完成路径规划与轨迹跟踪，针对在线计算量大的痛点，现有研究提出数值加速算法、分布式分解求解等优化手段。但非线性模型预测控制存在难以弥补的理论缺陷：有限预测时域下，优化目标最优无法等价保证闭环系统稳定，标准稳定证明手段依赖系统局部线性化，而曲线轨迹跟踪场景线性化假设完全失效，缺少完备的稳定性理论支撑，极大限制了 NMPC 在高精度水下航行器中的实际落地。

1.3 现有研究存在的关键问题

综合现有文献成果，当前 AUV 轨迹跟踪控制仍存在四点亟待解决的核心痛点：

1. 传统非线性连续控制器无法显式处理推进器推力饱和约束，极限工况下易出现控制输出超限、系统振荡；
2. 标准非线性模型预测控制缺少不依赖局部线性化的稳定证明体系，无法量化系统稳定吸引范围；
3. 推力分配与轨迹跟踪控制分层设计，两层控制器独立优化易出现推力输出冲突，跟踪性能存在损失；
4. 嵌入式水下控制器算力有限，现有优化控制算法对求解精度要求严苛，迭代次数受限后极易出现系统失稳。

1.4 本文研究内容与创新点

针对上述现存问题，本文以水平面观测级 AUV 为研究对象，提出基于李雅普诺夫理论的模型预测控制框架，完整解决带约束非线性航行器轨迹跟踪的稳定与性能优化问题，本文三大核心创新如下：

1. 一体化 LMPC 控制框架设计：将推力分配子问题整合进滚动时域优化目标，实现轨迹跟踪、推力分配同步求解，依靠在线优化充分利用推进器输出上限，大幅提升动态跟踪性能与抗扰动能力；
2. 无局部线性化稳定理论推导：借助反步控制构造李雅普诺夫收缩约束，规避标准 MPC 的线性化依赖，严格推导优化问题递推可行的充分条件，解析量化系统全局保证吸引域；
3. 兼容次优解的优化架构：控制器稳定性、可行性不受优化求解精度影响，仅跟踪性能随求解精度变化，可根据水下硬件算力灵活调整优化迭代次数，实现计算成本与控制效果的动态折中。

1.5 论文整体结构安排

本文各章节内容安排如下：第二章建立目标 AUV 的完整运动学、动力学数学模型，梳理模型固有正定、斜对称等关键数学性质，为控制器设计提供理论基础；第三章构建融合推力分配的 LMPC 滚动时域优化问题，给出完整离散滚动控制执行流程；第四章基于反步法设计辅助李雅普诺夫镇定控制器，完成优化问题递推可行性、闭环系统渐近稳定性完整理论证明；第五章设置两组典型曲线轨迹，在无扰动、强模型失配叠加海流扰动两类工况下完成仿真对比，量化分析 LMPC 与传统反步控制的跟踪误差、动态响应、抗扰性能差异；第六章总结全文研究成果，规划后续实体样机试验与算法融合优化的研究方向。

二、水下航行器系统建模与模型特性分析

2.1 试验平台与运动简化说明

本文选取 Saab SeaEye Falcon 观测型水下航行器作为研究对象，该装备搭载四台固定方位推进器，推进器布局仅能实现水平面纵向、横向平移与艏向回转运动，无法主动补偿航行过程中产生的横摇、纵倾姿态角，因此本文仅针对水平面二维轨迹跟踪问题开展建模与控制器设计，忽略垂向、横摇、纵倾三维自由度运动。

为完整描述航行器运动状态，建立两套正交参考坐标系：一是惯性大地坐标系，用于表征航行器全局平面位置与艏向角；二是固连于航行器重心的载体坐标系，用于描述航行器自身相对纵向、横向、艏向角速度。航行器完整运动模型分为坐标变换运动学模型、力与角速度耦合动力学模型两层，同时结合推进器推力分配关系，整合得到统一六维状态非线性状态方程。

2.2 航行器运动学与动力学模型

运动学模型用于描述大地坐标系位置姿态与载体坐标系角速度之间的坐标转换关系，依靠旋转矩阵完成两组坐标系的映射，能够直观表征航行器位置变化速率与自身运动速度的耦合关系。

动力学模型基于牛顿刚体运动定律建立，完整包含航行器本体惯性质量、水下流体附加惯性质量、科氏向心力与离心力、线性与非线性流体阻尼、水下静恢复力、推进器广义推力六部分物理项。其中惯性矩阵具备对称正定有界特性，科氏力矩阵满足斜对称特性，阻尼矩阵恒正定，静恢复力幅值存在固定上界，以上固有数学性质是后续李雅普诺夫稳定性证明的关键理论支撑。

四台推进器输出推力通过固定分配矩阵线性映射为航行器纵向、横向、艏向三维广义控制力与力矩，结合运动学、动力学微分关系，整合得到航行器完整非线性状态演化模型，模型输入为四台推进器输出推力，状态包含平面位置、艏向角、三轴角速度六类物理量。

2.3 模型固有关键数学性质总结

通过对完整动力学模型矩阵项的数学推导，总结六条不随航行运动状态变化的固有性质：

1. 包含附加质量的惯性矩阵对称正定，所有矩阵元素存在统一上界；
2. 科氏向心力矩阵满足斜对称特性，矩阵与其转置相加为零矩阵；
3. 姿态旋转矩阵的逆矩阵等于自身转置矩阵，坐标变换过程不改变速度向量二范数大小；
4. 流体阻尼矩阵恒正定，保证航行无外力输入时运动速度会自然衰减；
5. 推力分配矩阵的乘积矩阵可逆，保证任意目标广义推力力矩均可通过推进器推力组合实现；
6. 水下静恢复力的输出幅值存在全局固定上界，不会随航行位置无限增大。

上述性质全程贯穿控制器设计、可行性推导、稳定性证明全过程，能够大幅简化李雅普诺夫函数导数的放缩推导，为理论证明提供严谨数学支撑。

三、基于李雅普诺夫模型预测的轨迹跟踪控制框架

3.1 光滑参考轨迹与全维度参考状态构造

本文假设目标跟踪平面轨迹及其一至三阶时间导数连续光滑、全局有界，该假设符合水下巡航、管线跟踪等实际任务的轨迹设计标准。仅给出二维平面位置参考无法匹配航行器六维状态模型，因此基于位置轨迹导数构造完整参考状态序列，包含期望艏向角、期望纵向速度、期望回转角速度，横向参考速度固定为零，匹配水平面常规巡航运动需求。

结合航行器动力学模型反向推导跟踪参考所需的广义推力力矩，再通过推力分配矩阵的摩尔 - 彭罗斯伪逆求解四台推进器对应的参考推力输入，得到完整参考控制量。从数学角度可严格证明，在轨迹光滑有界假设下，全部参考状态、参考控制量均存在全局幅值上界，为后续稳定性证明中变量放缩提供基础条件。

3.2 标准非线性模型预测控制及其缺陷

传统非线性模型预测控制以跟踪误差、控制输入代价加权积分为优化目标，约束包含系统状态演化方程、初始状态匹配条件、推进器推力饱和幅值限制。通过滚动时域求解有限区间内最优控制序列，仅执行第一个采样周期控制量，下一时刻重新采集状态更新优化问题，实现闭环滚动控制。

该基础架构存在无法规避的理论短板：有限长度预测时域下，优化目标函数最小化仅能保证有限区间内性能最优，无法直接推导闭环系统全局稳定；现有稳定改进方案均需要在目标轨迹工作点做系统局部线性化，构造辅助线性镇定控制器并添加终端约束，而连续曲线跟踪过程中航行器工作点持续大范围变化，局部线性化假设完全失效，终端约束设计不再具备理论有效性，无法保证航行器跟踪曲线轨迹时的系统稳定。

3.3 带李雅普诺夫收缩约束的 LMPC 优化问题

为解决标准 NMPC 稳定缺陷，本文在原有优化约束基础上新增李雅普诺夫收缩不等式约束，构建全新 LMPC 优化架构。收缩约束依托稳定的反步辅助控制器与对应李雅普诺夫函数设计，其核心物理含义为：优化求解得到的控制输入，对应的李雅普诺夫函数下降速率至少不弱于辅助镇定控制器的收敛速率。

优化目标保留状态跟踪误差、控制推力幅值加权积分项与终端误差惩罚项，约束完整包含航行器非线性状态演化方程、初始状态匹配条件、推进器输出饱和上限、李雅普诺夫收缩约束四组条件。收缩约束使得滚动优化过程天然继承辅助反步控制器的全局稳定特性，无需对系统做任何线性近似处理，完美适配大曲率曲线轨迹跟踪场景。

3.4 LMPC 滚动时域实时控制算法

本文设计离散采样滚动执行算法，完整控制执行流程分为五步：第一步提前离线确定优化目标加权矩阵、预测时域长度、采样周期、推进器推力上限等固定控制器参数；第二步实时采集水下航行器当前全部六维运动状态；第三步以当前实测状态为初始条件，求解带收缩约束的 LMPC 优化问题，得到一段有限时域内推进器推力控制序列，算法允许输出局部次优解，不强制要求全局最优；第四步仅截取控制序列第一个采样周期的推力指令下发至推进器执行；第五步等待一个采样周期后重新采集航行器状态，循环迭代完成全程轨迹跟踪控制。

算法配套补充说明：优化求解的全局最优解仅影响轨迹跟踪精度，不会改变系统可行性与闭环稳定性。该特性适配水下嵌入式硬件算力受限的工程场景，操作人员可根据硬件计算速度限制优化迭代次数，牺牲少量跟踪精度换取更快在线求解速度，不会引发航行器失稳、振荡等安全问题，大幅提升算法工程落地适配性。

四、LMPC 控制器递推可行性与闭环稳定性理论分析

4.1 基于反步法的辅助李雅普诺夫镇定控制器设计

为构造 LMPC 优化所需的收缩约束，本文采用反步设计法设计无约束辅助跟踪控制器，该控制器仅用于稳定性证明，实际航行过程中不会下发至推进器执行。

首先定义航行器平面位置跟踪误差、速度误差变换变量，分步构造两层李雅普诺夫函数，第一层李雅普诺夫函数仅针对平面位置跟踪误差，第二层复合李雅普诺夫函数融合位置误差与速度误差项。对复合李雅普诺夫函数求时间导数，代入航行器完整动力学模型，借助模型斜对称、正定固有性质化简导数表达式，反向推导得到广义推力力矩控制律，再通过推力分配伪逆映射得到推进器辅助推力指令。

数学推导可证明，该反步辅助控制器能够使航行器轨迹跟踪误差全局渐近收敛至零，李雅普诺夫函数导数恒小于等于零，具备完备全局镇定能力，可作为收缩约束的理论基准控制器。

4.2 LMPC 优化问题递推可行性证明

递推可行性是滚动时域控制稳定运行的前提，代表每一个采样时刻，优化问题均存在满足全部约束的可行控制解。本文假设四台推进器输出幅值上限完全一致，结合推力分配矩阵范数有界特性，推导广义推力力矩上限与单台推进器推力上限的映射关系，给出推力分配始终满足饱和约束的充分条件。

基于反步辅助控制器的全局有界特性，证明任意航行初始状态下，辅助控制器输出推力始终满足推进器饱和约束，代表优化问题天然存在一组可行初始解。进一步推导航行状态、参考轨迹全部有界条件下，科氏矩阵、阻尼矩阵的全局幅值上界，结合反步控制增益参数，完整给出保证优化问题全程递推可行的充分不等式条件，同时量化给出系统稳定保证吸引域的显式表达范围。只要航行初始状态落在该吸引域内部，全程滚动优化均存在可行解，控制器可持续稳定工作。

4.3 闭环系统全局渐近稳定性证明

依托辅助反步控制器对应的径向无界、连续可微李雅普诺夫函数，结合非线性系统逆李雅普诺夫定理，存在一组标准无穷阶类 K 函数，能够对李雅普诺夫函数及其导数做上下界放缩。

LMPC 优化内部的收缩约束保证，每一步滚动优化得到的实际控制输入，都会使李雅普诺夫函数下降速率优于辅助镇定控制器。结合辅助控制器的负定导数特性，可直接推导出闭环系统李雅普诺夫函数导数严格负定，满足全局渐近稳定判定条件。

由此可完整证明：在轨迹光滑有界、推进器推力上限满足可行性条件前提下，本文 LMPC 滚动控制算法驱动的 AUV 闭环系统全局渐近稳定，航行器平面位置、艏向角、三轴角速度跟踪误差将持续收敛至零，最终稳定跟随目标参考轨迹。同时可得到工程优化结论：减小反步辅助控制器的增益矩阵数值，能够进一步扩大系统稳定吸引域，兼顾大范围初始偏差跟踪与系统稳定。

五、仿真试验与性能对比分析

5.1 仿真试验参数与测试工况设置

所有仿真试验均基于 Saab SeaEye Falcon 航行器实测辨识动力学模型搭建，设置两组差异化目标参考轨迹，分别对应低速平滑巡航、高曲率复杂机动两类典型水下任务场景：第一组为单频正弦平滑轨迹，用于常规匀速巡航工况测试；第二组为双频组合八字形高曲率轨迹，用于大角度连续转弯机动工况测试。

统一设置 LMPC 控制器采样周期、预测时域长度、误差加权矩阵、控制推力惩罚矩阵、终端误差惩罚矩阵，单台推进器最大输出推力统一设置为 500 牛；对比对照组采用同增益参数的传统反步跟踪控制器，两组控制器初始航行状态完全一致，保证对比试验公平性。

仿真分为两大测试工况：工况一为理想无扰动环境，无模型参数偏差、无外部洋流干扰，用于对比基础动态收敛速度、稳态跟踪误差；工况二为强鲁棒性测试工况，叠加 30% 全维度动力学模型参数失配，同时施加恒定横向、纵向洋流推力扰动，模拟真实复杂水下作业环境，验证两种控制器的抗干扰能力。优化求解采用序列二次规划算法，离散化后求解优化问题的 KKT 最优条件。

5.2 无扰动工况跟踪性能对比

理想无扰动环境下，传统反步控制器与本文 LMPC 控制器均能够最终收敛至目标参考轨迹，验证两类控制器的基础稳定能力，但动态跟踪性能存在显著差距。

从轨迹平面曲线可以直观看出，LMPC 控制器收敛速度远快于传统反步控制，初始位置偏差消除耗时大幅缩短；跟踪初期 LMPC 控制器会充分利用推进器最大输出推力快速抵消跟踪误差，推力指令全程严格控制在饱和上限以内，不存在超限振荡；传统反步控制器受固定增益限制，误差衰减缓慢，轨迹跟踪滞后现象明显。

在八字形高曲率轨迹转弯拐点处，反步控制器跟踪滞后误差显著增大，而 LMPC 依靠滚动时域提前预判轨迹变化，提前调整推进器推力，拐点处跟踪偏差始终维持在极低水平，复杂机动场景下优势更加突出。

5.3 强扰动工况鲁棒性能定量分析

叠加模型参数失配与海流扰动后，传统反步控制器跟踪性能大幅衰减，固定增益无法自适应补偿外部扰动与模型偏差，平面位置、艏向角持续存在较大稳态跟踪误差；本文 LMPC 控制器依靠滚动时域在线迭代优化，能够实时根据当前跟踪误差动态调整推进器推力，持续抵消未知扰动带来的偏差，全程维持高精度跟踪效果。

通过位置、艏向跟踪均方误差量化对比，正弦轨迹工况下 LMPC 位置跟踪误差降低幅度最高达到 94.9%，艏向角误差降低 94.3%；八字形复杂轨迹工况下横向位置均方误差降低 99.5%，跟踪精度提升幅度极为显著。数据充分证明，滚动时域优化架构天然赋予 LMPC 控制器更强的鲁棒性，更适配存在模型不确定性、洋流扰动的真实水下作业环境。

六、结论与后续研究展望

6.1 全文研究结论

本文针对水平面自主水下航行器的高精度轨迹跟踪控制难题，提出一套融合李雅普诺夫稳定理论与模型预测控制的 LMPC 一体化控制方案，完整完成建模、控制器设计、稳定性理论证明与多工况仿真验证，核心研究结论总结如下：

1. 所提 LMPC 框架可将推力分配优化嵌入滚动时域优化流程，实现轨迹跟踪、推力分配同步求解，能够直接显式处理推进器推力饱和等工程硬约束，充分利用推进器输出极限提升动态响应速度；
2. 依托反步控制构造李雅普诺夫收缩约束，彻底摆脱标准模型预测控制依赖局部线性化的稳定证明局限，完整推导优化问题递推可行、闭环系统渐近稳定的充分条件，量化给出系统稳定吸引域范围，理论完备性优于现有非线性 MPC 方案；
3. 算法兼容优化次优解，控制器稳定与可行性不受优化求解精度影响，仅跟踪性能随求解迭代次数变化，可灵活适配水下嵌入式低算力硬件，工程实用性更强；
4. 多组仿真对比试验证明，无论理想无扰动环境还是强模型失配叠加洋流扰动的复杂工况，LMPC 控制器相较于传统反步控制收敛速度更快、稳态跟踪误差更小，鲁棒跟踪性能具备压倒性优势，可满足各类高精度水下巡航作业需求。

6.2 后续研究方向规划

基于本文现有理论与仿真成果，后续将从实体样机试验、算法融合优化、轻量化改进三个维度开展深入研究：

1. 实体样机水池与湖试验证：依托 Saab SeaEye Falcon 实体水下航行器搭建试验平台，将本文 LMPC 算法移植至嵌入式控制器，通过水池、湖上实船试验验证算法在真实硬件、真实水流环境下的实际控制效果，完善工程参数整定方案；
2. 多控制算法融合优化：结合滑模控制、鲁棒 H∞控制、模糊自适应控制等先进非线性控制理论，对 LMPC 框架进行改进，进一步强化系统对时变扰动、未知大模型失配的抑制能力；
3. 在线优化轻量化改进：针对水下嵌入式硬件算力瓶颈，研究分布式分解、快速数值迭代求解算法，降低 LMPC 在线优化的计算耗时，适配小型低成本观测级 AUV 的实时控制需求。

📚第二部分——运行结果

原文：

主函数代码：

%% ------------------------------------------------------------------------ % 代码功能：基于线性模型预测控制(LMPC)的自主水下航行器(AUV)轨迹跟踪仿真 % 对比两种控制器：非线性反步控制器(Nonlinear BS Control)、LMPC控制器 % 可选择参考轨迹：二维正弦轨迹 / 八字形轨迹 % 输出曲线：跟踪轨迹、控制力、推进器推力、位置艏向时序、均方误差MSE指标 % ------------------------------------------------------------------------- % 添加工具函数文件夹路径，包含AUV模型、控制器、轨迹生成类定义 addpath('./utilis/','./ch4header/') clc; % 清空命令行窗口 clear; % 清空工作区所有变量 close all; % 新增：关闭所有已有绘图窗口，防止多图重叠混淆 %======================================================================% % 一、AUV动力学系统参数配置 %======================================================================% err_model=0; % 模型失配系数，0代表无模型不确定性，可设非零模拟参数扰动 m=116; % AUV本体质量(kg) Iz=13.1; % AUV绕z轴转动惯量(kg·m²) % 附加质量相关水动力系数 (附加质量矩阵对角线元素) X_udot=-167.6*(1+err_model); % 纵向附加质量系数 Y_vdot=-477.2*(1+err_model); % 横向附加质量系数 N_rdot=-15.9*(1+err_model); % 艏向转动附加惯量系数 % 线性阻尼水动力系数 Xu=26.9*(1+err_model); % 纵向线性阻尼 Yv=35.8*(1+err_model); % 横向线性阻尼 Nr=3.5*(1+err_model); % 艏向转动线性阻尼 % 二次阻尼水动力系数 Du=241.3*(1+err_model); % 纵向二次阻尼 Dv=503.8*(1+err_model); % 横向二次阻尼 Dr=76.9*(1+err_model); % 艏向转动二次阻尼 % 广义质量矩阵对角元素 (M = diag([Mx, My, Mψ])) Mx=m-X_udot; % 纵向等效质量 My=m-Y_vdot; % 横向等效质量 Mpsi=Iz-N_rdot; % 艏向等效转动惯量 % 推进器分配矩阵B：4个推进器输出映射为3维控制力矩[τ_u,τ_v,τ_r] % 维度：3行4列，输入4个推进器推力u1~u4，输出纵向力、横向力、艏向力矩 B=[0.7974 0.8643 0.8127 0.8270 0.6032 0.5029 -0.5824 -0.5610 0.2945 -0.3302 -0.2847 0.3505]; Bplus=pinv(B); % 求B的伪逆矩阵，由期望控制力矩反解4个推进器推力 M=diag([Mx;My;Mpsi]);% 广义质量矩阵，3×3对角阵 PI=[0,-1,0;1,0,0;0,0,0];% 坐标转换斜对称矩阵，用于机体-大地坐标系转换 g=0; % 重力/浮力项，水平面运动置0 Lambda=diag([1;1;1]);% 权重对角矩阵，本例未启用 % 外部环境扰动向量 [纵向扰动力,横向扰动力,艏向扰动力矩] % disturbance=[10;10;0]; % 带扰动工况 disturbance=[0;0;0]; % 无扰动仿真工况 %======================================================================% % 二、创建两台AUV仿真对象 % auv1：用于非线性反步控制器仿真 % auv2：用于LMPC控制器仿真 %======================================================================% coef = [m; Iz; X_udot; Y_vdot; N_rdot; Xu; Yv; Nr; Du; Dv; Dr];% 整合所有水动力参数向量 ndof = 3; % 自由度：水平面3自由度(纵移u,横移v,艏摇r) X0=[0.5;0;0;0;0;0]; % AUV初始状态向量 [x,y,ψ,u,v,r] Thrust0 = [0;0;0;0]; % 初始4推进器推力全为0 auv1 = AUV(coef, ndof, B, X0, Thrust0);% 反步控制仿真载体 auv2 = AUV(coef, ndof, B, X0, Thrust0);% LMPC控制仿真载体 %======================================================================% % 三、初始化非线性反步控制器(BS Controller) %======================================================================% Kp=diag([1;1;1]); % 比例增益矩阵 位置环P增益 Kd=diag([1;1;1]); % 微分增益矩阵 速度环D增益 bs1 = nonlinearBS_controller(Kp,Kd);% 实例化反步控制器对象 %======================================================================% % 四、生成参考轨迹对象（二选一） %======================================================================% % Case 1: 二维正弦参考轨迹，参数(幅值,周期,初始相位) ref_trajectory = sine2D(1,0.5,0); % Case 2: 八字形参考轨迹，参数(幅值,纵向周期,横向缩放,初始相位) % ref_trajectory = eight_shape2D(1,0.5,0.25,0); %======================================================================% % 五、非线性反步控制 完整仿真循环 %======================================================================% % 参考轨迹信息向量：9维 [x,y,ψ,dx,dy,dψ,ddx,ddy,ddψ] 位置/速度/加速度 ref = [ref_trajectory.xR;ref_trajectory.yR;ref_trajectory.psiR;... ref_trajectory.xRd;ref_trajectory.yRd;ref_trajectory.psiRd;... ref_trajectory.xRdd;ref_trajectory.yRdd;ref_trajectory.psiRdd]; dt=0.1; % 仿真步长 0.1s Tstep=200; % 总仿真步数200步，总时长200*0.1=20s nx=length(X0); % 状态维度6维 [x,y,ψ,u,v,r] nu=3; % 虚拟控制输入维度3维 [τu,τv,τr] n_thrust=4; % 物理推进器数量4个 % 预分配存储矩阵，加速仿真 Xall=zeros(nx,Tstep+1);% auv1全部状态时序 (6行201列) Uall=zeros(nu,Tstep); % 反步控制器输出虚拟力矩 (3行200列) Thrust_all=zeros(n_thrust,Tstep);% 反步控制下4推进器实际推力(4行200列) eta_Ref_all=zeros(9,Tstep);% 全时序参考轨迹9维信息 (9行200列) Xall(:,1)=X0; % 仿真初始状态赋值 Xplus=X0; % 反步控制仿真循环 for i=1:1:Tstep eta_Ref_all(:,i) = ref;% 保存当前时刻参考轨迹 tau = bs1.calc_control(ref,auv1.X);% 反步控制器计算期望控制力矩τ Uall(:,i)= tau; % 记录虚拟控制力矩 thrust = Bplus*tau; % 伪逆分配：力矩转4推进器推力 Thrust_all(:,i) = thrust; % 保存推进器推力 auv1.advance(thrust,disturbance,dt);% AUV动力学一步积分更新状态 Xall(:,i+1) = auv1.X; % 保存更新后AUV状态 ref_trajectory.update(dt); % 更新参考轨迹时间，生成下一时刻轨迹 ref_trajectory.t; % 读取当前轨迹时间(仅查看无赋值) % 刷新下一时刻完整参考轨迹9维数据 ref = [ref_trajectory.xR;ref_trajectory.yR;ref_trajectory.psiR;... ref_trajectory.xRd;ref_trajectory.yRd;ref_trajectory.psiRd;... ref_trajectory.xRdd;ref_trajectory.yRdd;ref_trajectory.psiRdd]; end % 绘图1：先绘制参考轨迹与反步控制轨迹(后续LMPC曲线叠加) figure(1) plot(eta_Ref_all(1,:),eta_Ref_all(2,:),'k'); hold on; plot(Xall(1,:),Xall(2,:),'b'); hold on; %======================================================================% % 六、LMPC模型预测控制器参数初始化 %======================================================================% N=5; % LMPC预测时域长度5步 Tstep2 = Tstep-N; % LMPC有效仿真步数，扣除预测时域前置步数 % 状态代价权重矩阵Q：6维状态[x,y,ψ,u,v,r]权重 q11=1e5; % x位置跟踪权重(最大，优先保证位置跟踪) q22=1e5; % y位置跟踪权重 q33=1e3; % 艏向角ψ跟踪权重 q44=1e2; % 纵向速度u权重 q55=1e2; % 横向速度v权重 q66=1e2; % 艏摇角速度r权重 Q=diag([q11,q22,q33,q44,q55,q66]); % 控制输入代价权重R：4个推进器推力权重 r11=1e-4; r22=1e-4; r33=1e-4; r44=1e-4; R=diag([r11,r22,r33,r44]); R1=0; % 备用控制权重，本代码未使用 % 终端代价权重Qf：预测时域终点状态惩罚权重 qf11=1e3; qf22=1e3; qf33=1e2; qf44=1e1; qf55=1e1; qf66=1e1; Qf=diag([qf11,qf22,qf33,qf44,qf55,qf66]); weights = {Q,R,Qf}; % 打包代价权重传入LMPC类 % 推进器推力上下限约束 T1_max = 500; T2_max = 500; T3_max = 500; T4_max = 500; Thrust_max0 = [T1_max;T2_max;T3_max;T4_max];% 推力上限 Thrust_min0 = [-T1_max;-T2_max;-T3_max;-T4_max];% 推力下限(反向推力) internal_model = AUV(coef, ndof, B, X0, Thrust0);% LMPC内部预测AUV模型 auxiliary_controller = nonlinearBS_controller(Kp,Kd);% LMPC初始可行解辅助反步控制器 lmpc1 = LMPC_controller(N,internal_model,auxiliary_controller,weights,Thrust_max0,Thrust_min0);% 实例化LMPC控制器 %======================================================================% % 七、LMPC控制器仿真循环 %======================================================================% Xall2 = zeros(nx,Tstep2+1); % LMPC控制下AUV全状态时序 Uall2 = zeros(nu,Tstep2); % LMPC输出虚拟控制力矩τ Thrust_all2 = zeros(n_thrust,Tstep);% LMPC下4推进器推力时序 Xall2(:,1) = X0; % LMPC仿真初始状态 U0 = Uall2(:,1); Thrust0 = Bplus*U0; % 初始推力 M = nx + nu + n_thrust; % 参考轨迹生成函数参数维度 t = 0; % 仿真时间计数器 u0 = zeros(n_thrust*N,1); % LMPC优化初始猜测序列(预测时域内所有推力) tic; % 计时，统计LMPC仿真总耗时 % LMPC主仿真循环 for i=1:1:Tstep2 t1 = t; TIme(i,1) = t; % 记录每一步仿真时间戳 P = RefGen(M,N,t1,dt,coef,Bplus,ref_trajectory);% 生成预测时域内参考轨迹序列 u = lmpc1.calc_control(P,X0,B,u0,dt,ref_trajectory,t1);% LMPC求解优化问题，得到预测时域推力序列 u_actual = u(1:n_thrust,1);% 取预测序列第一个推力作为当前实际控制量 Thrust_all2(:,i) = u_actual;% 保存当前推进器推力 tau_actual = B*u_actual; % 推力映射为AUV控制力矩τ Uall2(:,i) = tau_actual; % 保存虚拟控制力矩 auv2.advance(u_actual,disturbance,dt);% AUV动力学更新状态 Xall2(:,i+1) = auv2.X; % 记录更新后状态 X0 = auv2.X; % 刷新当前状态给下一时刻LMPC % 基于反步控制器生成下一时刻优化初始可行猜测解，加速fmincon求解 t2 = t+dt; P_ = RefGen(M,N,t2,dt,coef,Bplus,ref_trajectory); u0 = lmpc1.calc_initial_guess(P_,X0,Bplus,dt); i; % 控制台输出当前循环步数，观测仿真进度 t = t + dt; % 仿真时间向前推进一个步长 end toc; % 输出LMPC仿真总耗时 %========================================================================== % 八、结果绘图模块：多维度对比两种控制器效果 %========================================================================== %% 图1：水平面x-y轨迹对比 figure(1) plot(eta_Ref_all(1,:),eta_Ref_all(2,:),'k','LineWidth',1.2); hold on; plot(Xall(1,1:Tstep2),Xall(2,1:Tstep2),'b','LineWidth',1); hold on; plot(Xall2(1,1:Tstep2),Xall2(2,1:Tstep2),'r','LineWidth',1); grid on; legend('参考轨迹Ref', '非线性反步控制BS', 'LMPC模型预测控制','Location','best'); xlabel('纵向位置 x [m]'); ylabel('横向位置 y [m]'); title('AUV水平面轨迹跟踪对比'); %% 图2：虚拟控制力矩τ时序对比(纵向力、横向力、艏向力矩) figure(2) subplot(3,1,1) plot(TIme(1:Tstep2,1),Uall(1,1:Tstep2),'b'); hold on; plot(TIme(1:Tstep2,1),Uall2(1,1:Tstep2),'r'); grid on; ylabel('纵向控制力 F_u [N]'); legend('BS反步','LMPC预测','Location','best'); title('控制器输出虚拟控制力矩时序'); subplot(3,1,2) plot(TIme(1:Tstep2,1),Uall(2,1:Tstep2),'b'); hold on; plot(TIme(1:Tstep2,1),Uall2(2,1:Tstep2),'r'); grid on; ylabel('横向控制力 F_v [N]'); subplot(3,1,3) plot(TIme(1:Tstep2,1),Uall(3,1:Tstep2),'b'); hold on; plot(TIme(1:Tstep2,1),Uall2(3,1:Tstep2),'r'); grid on; xlabel('仿真时间 Time [s]'); ylabel('艏向控制力矩 F_r [Nm]'); %% 图3：4个推进器实际推力时序对比 figure(3) subplot(4,1,1) plot(TIme(1:Tstep2,1),Thrust_all(1,1:Tstep2),'b'); hold on; plot(TIme(1:Tstep2,1),Thrust_all2(1,1:Tstep2),'r'); grid on; ylabel('推进器1推力 u_1 [N]') title('四台推进器输出推力时序对比'); subplot(4,1,2) plot(TIme(1:Tstep2,1),Thrust_all(2,1:Tstep2),'b'); hold on; plot(TIme(1:Tstep2,1),Thrust_all2(2,1:Tstep2),'r'); grid on; ylabel('推进器2推力 u_2 [N]') subplot(4,1,3) plot(TIme(1:Tstep2,1),Thrust_all(3,1:Tstep2),'b'); hold on; plot(TIme(1:Tstep2,1),Thrust_all2(3,1:Tstep2),'r'); grid on; ylabel('推进器3推力 u_3 [N]') subplot(4,1,4) plot(TIme(1:Tstep2,1),Thrust_all(4,1:Tstep2),'b'); hold on; plot(TIme(1:Tstep2,1),Thrust_all2(4,1:Tstep2),'r'); grid on; ylabel('推进器4推力 u_4 [N]') xlabel('仿真时间 Time [s]'); %% 图4：位置x、y、艏向角ψ时序跟踪曲线 figure(4) subplot(3,1,1) plot(TIme(1:Tstep2,1),eta_Ref_all(1,1:Tstep2),'k'); hold on; plot(TIme(1:Tstep2,1),Xall(1,1:Tstep2),'b'); hold on; grid on; plot(TIme(1:Tstep2,1),Xall2(1,1:Tstep2),'r'); ylabel('纵向位置 x [m]'); title('位置与艏向角跟踪时序对比'); subplot(3,1,2) plot(TIme(1:Tstep2,1),eta_Ref_all(2,1:Tstep2),'k'); hold on; plot(TIme(1:Tstep2,1),Xall(2,1:Tstep2),'b'); hold on; grid on; plot(TIme(1:Tstep2,1),Xall2(2,1:Tstep2),'r'); ylabel('横向位置 y [m]'); subplot(3,1,3) plot(TIme(1:Tstep2,1),eta_Ref_all(3,1:Tstep2),'k'); hold on; plot(TIme(1:Tstep2,1),Xall(3,1:Tstep2),'b'); hold on; grid on; plot(TIme(1:Tstep2,1),Xall2(3,1:Tstep2),'r'); xlabel('仿真时间 Time [s]'); ylabel('艏向角 \psi [rad]'); %% 图5（注释备用）：速度u,v,r时序曲线，取消注释即可绘图 % figure(5) % subplot(3,1,1) % plot(TIme(1:Tstep2,1),Xall(4,1:Tstep2),'b'); % hold on; % grid on; % plot(TIme(1:Tstep2,1),Xall2(4,1:Tstep2),'r'); % title('纵向速度u时序对比'); % % subplot(3,1,2) % plot(TIme(1:Tstep2,1),Xall(5,1:Tstep2),'b'); % hold on; % grid on; % plot(TIme(1:Tstep2,1),Xall2(5,1:Tstep2),'r'); % title('横向速度v时序对比'); % % subplot(3,1,3) % plot(TIme(1:Tstep2,1),Xall(6,1:Tstep2),'b'); % hold on; % grid on; % plot(TIme(1:Tstep2,1),Xall2(6,1:Tstep2),'r'); % title('艏摇角速度r时序对比'); % xlabel('Time [s]'); %======================================================================% % 九、计算两种控制器跟踪均方误差MSE，量化跟踪性能 %======================================================================% % 初始化x,y,ψ三个自由度均方误差累加器 MSEx1=0; MSEy1=0; MSEpsi1=0; % BS反步控制误差累加 MSEx2=0; MSEy2=0; MSEpsi2=0; % LMPC控制误差累加 for i=1:1:Tstep2 % x方向误差平方累加 MSEx1=MSEx1+(Xall(1,i)-eta_Ref_all(1,i))^2; MSEx2=MSEx2+(Xall2(1,i)-eta_Ref_all(1,i))^2; % y方向误差平方累加 MSEy1=MSEy1+(Xall(2,i)-eta_Ref_all(2,i))^2; MSEy2=MSEy2+(Xall2(2,i)-eta_Ref_all(2,i))^2; % 艏向角ψ误差平方累加 MSEpsi1=MSEpsi1+(Xall(3,i)-eta_Ref_all(3,i))^2; MSEpsi2=MSEpsi2+(Xall2(3,i)-eta_Ref_all(3,i))^2; end % 求均值，得到均方误差MSE MSEx1=MSEx1/Tstep2; MSEx2=MSEx2/Tstep2; MSEy1=MSEy1/Tstep2; MSEy2=MSEy2/Tstep2; MSEpsi1=MSEpsi1/Tstep2; MSEpsi2=MSEpsi2/Tstep2; % 命令行输出量化指标 fprintf('==========跟踪均方误差MSE对比==========\n'); fprintf('非线性反步控制 BS：Mx=%.4f, My=%.4f, Mψ=%.4f\n',MSEx1,MSEy1,MSEpsi1); fprintf('LMPC模型预测控制：Mx=%.4f, My=%.4f, Mψ=%.4f\n',MSEx2,MSEy2,MSEpsi2); fprintf('=======================================\n'); % 移除自定义工具文件夹路径，避免影响后续其他脚本 rmpath('./utilis/','./ch4header/')

🎉第三部分——参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。(文章内容仅供参考，具体效果以运行结果为准)

​​​​​​🌈第四部分——本文完整资源下载

资料获取，更多粉丝福利，MATLAB|Simulink|Python|数据|文档等完整资源获取

本文完整资源下载